diff --git a/CMakeLists.txt b/CMakeLists.txt
index b41b59591980fe3c1d6603dbb4448c308de9133d..308bddb12bbc1dd0968f0348b19024e6c6bf35c0 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index 53f1c25e6d7e3e6fb857ce02033a980752b18fcc..34d3506a8d9a815e1cc133d7b80d86ef6b335da0 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -6,6 +6,7 @@
"pse": {
"pse_costum_obstacle_penalties": true,
"pse_costum_obstacle_penalties_dynamic": true,
+ "weight_costmap": 0.5,
"weight_global_path_position": 0.3,
"weight_global_path_orientation": 0.3,
"lrk_use_alternative_approach": false,
diff --git a/examples/components/example_client/CMakeLists.txt b/examples/components/example_client/CMakeLists.txt
index 01f000d5e96b1135fb133149e40dfd3f5116c9d6..5d89c439dc41853cc00d1eb46377befa6b304206 100644
--- a/examples/components/example_client/CMakeLists.txt
+++ b/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
)
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index 4bb1c1f715bde306bde98dce9d7189edec591b22..11261fa1ff76b3d82c8e169f5d71ccdf0cc0e6c0 100644
--- a/examples/components/example_client/Component.cpp
+++ b/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;
}
diff --git a/examples/components/example_client/Component.h b/examples/components/example_client/Component.h
index 3c6748d3ea90401b3cc385715232ab73be814f00..f8b5bb836c7cc0c2bc3890ee9170747015dafb89 100644
--- a/examples/components/example_client/Component.h
+++ b/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
diff --git a/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx b/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx
index 570cffa4bdc3848ed6f32959981904d3bac7ecfc..942bdb1ba27bece4153f0b52b3b1c502cfedaf81 100644
--- a/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx
+++ b/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>
diff --git a/scenarios/HumanAwareNavigation/config/human_simulator.cfg b/scenarios/HumanAwareNavigation/config/human_simulator.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..c0a571fa5e9de5a86f9fecf99428a7334f2302f8
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/human_simulator.cfg
@@ -0,0 +1,278 @@
+# ==================================================================
+# 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
+
+
diff --git a/scenarios/PlatformNavigation/PlatformNavigation.scx b/scenarios/PlatformNavigation/PlatformNavigation.scx
index a290831fe89e739223449751650fa2784a105144..cc0fcea78f6ed640190fa44dd5e5ff33d17e4d16 100644
--- a/scenarios/PlatformNavigation/PlatformNavigation.scx
+++ b/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"/>
diff --git a/scenarios/PlatformNavigation/config/ObjectMemory.cfg b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
index be7ed81802e0055c94627a2d148b113c51af7e73..773e48120ca477d223c4745028bede338999bc49 100644
--- a/scenarios/PlatformNavigation/config/ObjectMemory.cfg
+++ b/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
diff --git a/scenarios/PlatformNavigation/config/example_client.cfg b/scenarios/PlatformNavigation/config/example_client.cfg
index f459b030d93e7be0f064894110909b6c538d98fd..9d58c81813cc02c46d7885c96914440a80814c7d 100644
--- a/scenarios/PlatformNavigation/config/example_client.cfg
+++ b/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
-
-
diff --git a/scenarios/PlatformNavigation/config/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index 74b205e76278fbbd36c35a4456875391562bbf45..751247e4a134226de1a9ad4c1e3b781c32a4acd5 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/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
-
-
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index 5bf206ad3cfe3c6b69da36e64f6653d1cea9c9af..36c97cb019ba704407f9f3b2bfd0e48d0c77eaf1 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/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
-
-
diff --git a/source/armarx/navigation/CMakeLists.txt b/source/armarx/navigation/CMakeLists.txt
index 6093698656d8c8cd7f22d1c2e8e560b60b932d61..ec9f87481512c4270242709250116f52ee2427c1 100644
--- a/source/armarx/navigation/CMakeLists.txt
+++ b/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)
diff --git a/source/armarx/navigation/algorithms/util.cpp b/source/armarx/navigation/algorithms/util.cpp
index fe71db5aeff5c090d9de9148fd366be374cb9158..6bc07a84c98b4c04390ed355ace3048f09955cc0 100644
--- a/source/armarx/navigation/algorithms/util.cpp
+++ b/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
diff --git a/source/armarx/navigation/algorithms/util.h b/source/armarx/navigation/algorithms/util.h
index ec8d9ae6c4c2bf18d4b3ead67c1c08f8804cfc9b..90c61b2620b6f52567b78da57378032da7e1c584 100644
--- a/source/armarx/navigation/algorithms/util.h
+++ b/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
diff --git a/source/armarx/navigation/client/services/EventSubscriptionInterface.h b/source/armarx/navigation/client/services/EventSubscriptionInterface.h
index 115babcfd11e8e8b6e33790402966c801ec7f4b9..e225f675c9dab1d8d7bda94dbf49a83eef7e3b70 100644
--- a/source/armarx/navigation/client/services/EventSubscriptionInterface.h
+++ b/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;
diff --git a/source/armarx/navigation/client/services/SimpleEventHandler.cpp b/source/armarx/navigation/client/services/SimpleEventHandler.cpp
index c849f30a69d6cc38e3fd25a18a2b11ef0280a441..8406a84023585f2e5b738b3615b721c06d8e0279 100644
--- a/source/armarx/navigation/client/services/SimpleEventHandler.cpp
+++ b/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)
{
diff --git a/source/armarx/navigation/client/services/SimpleEventHandler.h b/source/armarx/navigation/client/services/SimpleEventHandler.h
index 6bb0e690a482c2565145041059238a5e2fd757ec..a76a4a61a2094a581d24983c1cc961650ad9032d 100644
--- a/source/armarx/navigation/client/services/SimpleEventHandler.h
+++ b/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;
diff --git a/source/armarx/navigation/components/CMakeLists.txt b/source/armarx/navigation/components/CMakeLists.txt
index 317b56b4c6d115ac60ec7a21c86bac8b31a7f8fa..028e7732d578f24c008015f98574b0bffb731a53 100644
--- a/source/armarx/navigation/components/CMakeLists.txt
+++ b/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
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
index 6b89eb56af7abb5fed5b862ad200ef6e187f3f8d..5a317a1a2e4431a11c68f1edb401a299933c951d 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -45,8 +45,8 @@
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
-#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
// Include headers you only need in function definitions in the .cpp.
@@ -88,7 +88,9 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
def->required(properties.staticCostmap.providerName, "p.staticCostmap.providerName", "");
def->optional(properties.laserScannerFeatures.name, "p.laserScannerFeatures.name", "");
- def->optional(properties.laserScannerFeatures.providerName, "p.laserScannerFeatures.providerName", "");
+ def->optional(properties.laserScannerFeatures.providerName,
+ "p.laserScannerFeatures.providerName",
+ "");
def->optional(properties.robot.name, "p.robot.name", "");
@@ -172,13 +174,14 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
bool
Component::readStaticCostmap()
{
- const memory::client::costmap::Reader::Query query{
- .providerName = properties.staticCostmap.providerName,
- .name = properties.staticCostmap.name,
- .timestamp = armarx::core::time::Clock::Now()};
while (true)
{
+ const memory::client::costmap::Reader::Query query{
+ .providerName = properties.staticCostmap.providerName,
+ .name = properties.staticCostmap.name,
+ .timestamp = armarx::core::time::Clock::Now()};
+
const auto result = costmapReader.query(query);
if (result.costmap.has_value())
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 4a1cd5e85110c42c1a150ab8cdcb1e442d3826ee..23a58b26105a5987313dd793449c41e83f2c9ceb 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/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);
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index d63321007454a415b45c11d3a4e301ed86a8644f..ec7fca6e5d8ae0b53b2da443c037d4e35b1269c4 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -1,4 +1,3 @@
-
#include "HumanTracker.h"
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index adb3bad76bd2644fd894d4036d745f602cd198ce..5b015f7f672fe5c226d01e73f142c08909e3f281 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -1,5 +1,26 @@
-#pragma once
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @author Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
#include <ArmarXCore/core/time.h>
diff --git a/source/armarx/navigation/components/human_simulator/CMakeLists.txt b/source/armarx/navigation/components/human_simulator/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3ea4392a9fd911109b7f85046edec9d1c4d9469c
--- /dev/null
+++ b/source/armarx/navigation/components/human_simulator/CMakeLists.txt
@@ -0,0 +1,26 @@
+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
+)
diff --git a/source/armarx/navigation/components/human_simulator/Component.cpp b/source/armarx/navigation/components/human_simulator/Component.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3410013c006ce7d0ce85ed3a21629ff5f7430713
--- /dev/null
+++ b/source/armarx/navigation/components/human_simulator/Component.cpp
@@ -0,0 +1,325 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package navigation::ArmarXObjects::human_simulator
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "Component.h"
+
+#include <cstdlib>
+
+#include <VirtualRobot/Random.h>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/core/basic_types.h"
+#include "armarx/navigation/human/types.h"
+#include "armarx/navigation/simulation/SimulatedHuman.h"
+
+// Include headers you only need in function definitions in the .cpp.
+
+// #include <Eigen/Core>
+
+// #include <SimoxUtility/color/Color.h>
+
+
+namespace armarx::navigation::components::human_simulator
+{
+ Component::Component()
+ {
+ addPlugin(virtualRobotReaderPlugin);
+ addPlugin(costmapReaderPlugin);
+ addPlugin(humanWriterPlugin);
+ }
+
+
+ const std::string Component::defaultName = "human_simulator";
+
+
+ armarx::PropertyDefinitionsPtr
+ Component::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def =
+ new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+
+ // Publish to a topic (passing the TopicListenerPrx).
+ // def->topic(myTopicListener);
+
+ // Subscribe to a topic (passing the topic name).
+ // def->topic<PlatformUnitListener>("MyTopic");
+
+ // Use (and depend on) another component (passing the ComponentInterfacePrx).
+ // def->component(myComponentProxy)
+
+
+ // Add a required property. (The component won't start without a value being set.)
+ // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+
+ // Add an optionalproperty.
+ def->optional(properties.taskPeriodMs, "p.taskPeriodMs", "");
+ def->optional(properties.nHumans, "p.nHumans", "Number of humans to spawn.");
+
+ def->optional(properties.humanParams.goalDistanceThreshold, "p.human.goalDistanceThreshold", "");
+ def->optional(properties.humanParams.minLinearVelocity, "p.human.maxLinearVelocity", "");
+ def->optional(properties.humanParams.maxLinearVelocity, "p.human.maxLinearVelocity", "");
+
+ return def;
+ }
+
+
+ void
+ Component::onInitComponent()
+ {
+ // Topics and properties defined above are automagically registered.
+
+ // Keep debug observer data until calling `sendDebugObserverBatch()`.
+ // (Requies the armarx::DebugObserverComponentPluginUser.)
+ // setDebugObserverBatchModeEnabled(true);
+ }
+
+
+ void
+ Component::onConnectComponent()
+ {
+ // Do things after connecting to topics and components.
+
+ /* (Requies the armarx::DebugObserverComponentPluginUser.)
+ // Use the debug observer to log data over time.
+ // The data can be viewed in the ObserverView and the LivePlotter.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ */
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ // Draw boxes in ArViz.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ drawBoxes(properties, arviz);
+ */
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ // Setup the remote GUI.
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+ */
+
+
+ ///
+
+ //
+ // Costmaps
+ //
+
+ ARMARX_INFO << "Querying costmap";
+
+ 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();
+ };
+ }();
+
+ /// create humans
+
+ simulatedHumans.clear();
+
+ for (std::size_t i = 0; i < properties.nHumans; i++)
+ {
+ simulatedHumans.emplace_back(costmap);
+ }
+
+
+ ///
+
+ task = new PeriodicTask<Component>(
+ this, &Component::runPeriodically, properties.taskPeriodMs, false, "runningTask");
+ task->start();
+ }
+
+
+ void
+ Component::onDisconnectComponent()
+ {
+ task->stop();
+ }
+
+
+ void
+ Component::onExitComponent()
+ {
+ }
+
+
+ std::string
+ Component::getDefaultName() const
+ {
+ return Component::defaultName;
+ }
+
+
+ std::string
+ Component::GetDefaultName()
+ {
+ return Component::defaultName;
+ }
+
+ void
+ Component::runPeriodically()
+ {
+ // obtain data from perception
+
+ const DateTime timestamp = Clock::Now();
+
+ //
+ // Robot
+ //
+
+ // ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp));
+ // const core::Pose global_T_robot(robot->getGlobalPose());
+
+ // ARMARX_INFO << "Robot position: " << global_T_robot.translation().head<2>();
+
+
+ human::Humans humans;
+
+ for (auto& simulatedHuman : simulatedHumans)
+ {
+ humans.push_back(simulatedHuman.update());
+ }
+
+ ARMARX_VERBOSE << "Updating humans.";
+ humanWriterPlugin->get().store(humans, getName(), timestamp);
+ }
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ void
+ Component::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ // Setup the widgets.
+
+ tab.boxLayerName.setValue(properties.boxLayerName);
+
+ tab.numBoxes.setValue(properties.numBoxes);
+ tab.numBoxes.setRange(0, 100);
+
+ tab.drawBoxes.setLabel("Draw Boxes");
+
+ // Setup the layout.
+
+ GridLayout grid;
+ int row = 0;
+ {
+ grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
+ ++row;
+
+ grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
+ ++row;
+
+ grid.add(tab.drawBoxes, {row, 0}, {2, 1});
+ ++row;
+ }
+
+ VBoxLayout root = {grid, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+
+ void
+ Component::RemoteGui_update()
+ {
+ if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
+ {
+ std::scoped_lock lock(propertiesMutex);
+ properties.boxLayerName = tab.boxLayerName.getValue();
+ properties.numBoxes = tab.numBoxes.getValue();
+
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ }
+ if (tab.drawBoxes.wasClicked())
+ {
+ // Lock shared variables in methods running in seperate threads
+ // and pass them to functions. This way, the called functions do
+ // not need to think about locking.
+ std::scoped_lock lock(propertiesMutex, arvizMutex);
+ drawBoxes(properties, arviz);
+ }
+ }
+ */
+
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ void
+ Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
+ {
+ // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
+ // See the ArVizExample in RobotAPI for more examples.
+
+ viz::Layer layer = arviz.layer(p.boxLayerName);
+ for (int i = 0; i < p.numBoxes; ++i)
+ {
+ layer.add(viz::Box("box_" + std::to_string(i))
+ .position(Eigen::Vector3f(i * 100, 0, 0))
+ .size(20).color(simox::Color::blue()));
+ }
+ arviz.commit(layer);
+ }
+ */
+
+
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
+
+} // namespace armarx::navigation::components::human_simulator
diff --git a/source/armarx/navigation/components/human_simulator/Component.h b/source/armarx/navigation/components/human_simulator/Component.h
new file mode 100644
index 0000000000000000000000000000000000000000..52e7aadfee802ea9034d0f50d14195782118345e
--- /dev/null
+++ b/source/armarx/navigation/components/human_simulator/Component.h
@@ -0,0 +1,175 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package navigation::ArmarXObjects::human_simulator
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// #include <mutex>
+
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include <ArmarXCore/core/Component.h>
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+
+// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include "armarx/navigation/simulation/SimulatedHuman.h"
+#include "armarx/navigation/memory/client/costmap/Reader.h"
+#include "armarx/navigation/memory/client/human/Writer.h"
+#include <armarx/navigation/components/human_simulator/ComponentInterface.h>
+
+
+namespace armarx::navigation::components::human_simulator
+{
+
+ class Component :
+ virtual public armarx::Component,
+ virtual public armarx::navigation::components::human_simulator::ComponentInterface
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ // , virtual public armarx::ArVizComponentPluginUser
+ {
+ public:
+ Component();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ /// Get the component's default name.
+ static std::string GetDefaultName();
+
+
+ protected:
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+
+ /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// This function should be called once in onConnect() or when you
+ /// need to re-create the Remote GUI tab.
+ void createRemoteGuiTab();
+
+ /// After calling `RemoteGui_startRunningTask`, this function is
+ /// called periodically in a separate thread. If you update variables,
+ /// make sure to synchronize access to them.
+ void RemoteGui_update() override;
+ */
+
+
+ private:
+ // Private methods go here.
+
+ // Forward declare `Properties` if you used it before its defined.
+ // struct Properties;
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ /// Draw some boxes in ArViz.
+ void drawBoxes(const Properties& p, viz::Client& arviz);
+ */
+
+ void runPeriodically();
+
+
+
+ private:
+ static const std::string defaultName;
+
+ PeriodicTask<Component>::pointer_type task;
+
+
+
+ // Private member variables go here.
+
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ int taskPeriodMs = 100;
+
+
+ std::size_t nHumans = 4;
+
+ human::simulation::SimulatedHumanParams humanParams;
+ };
+ Properties properties;
+ /* Use a mutex if you access variables from different threads
+ * (e.g. ice functions and RemoteGui_update()).
+ std::mutex propertiesMutex;
+ */
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// Tab shown in the Remote GUI.
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ armarx::RemoteGui::Client::LineEdit boxLayerName;
+ armarx::RemoteGui::Client::IntSpinBox numBoxes;
+
+ armarx::RemoteGui::Client::Button drawBoxes;
+ };
+ RemoteGuiTab tab;
+ */
+
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ * When used from different threads, an ArViz client needs to be synchronized.
+ /// Protects the arviz client inherited from the ArViz plugin.
+ std::mutex arvizMutex;
+ */
+
+ template <typename T>
+ using ReaderWriterPlugin = armem::client::plugins::ReaderWriterPlugin<T>;
+
+
+ ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin =
+ nullptr;
+
+ ReaderWriterPlugin<memory::client::costmap::Reader>* costmapReaderPlugin = nullptr;
+
+
+ ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
+
+
+ std::vector<human::simulation::SimulatedHuman> simulatedHumans;
+
+
+ };
+
+} // namespace armarx::navigation::components::human_simulator
diff --git a/source/armarx/navigation/components/human_simulator/ComponentInterface.ice b/source/armarx/navigation/components/human_simulator/ComponentInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..338669c45f62829d4f64192364446629af030b7c
--- /dev/null
+++ b/source/armarx/navigation/components/human_simulator/ComponentInterface.ice
@@ -0,0 +1,35 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * package navigation::human_simulator
+ * author Fabian Reister ( fabian dot reister at kit dot edu )
+ * date 2022
+ * copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+module armarx { module navigation { module components { module human_simulator
+{
+
+ interface ComponentInterface
+ {
+ // Define your interface here.
+ };
+
+};};};};
diff --git a/source/armarx/navigation/components/navigation_memory/Component.h b/source/armarx/navigation/components/navigation_memory/Component.h
index fcbcb08d3dd05a1a4832eb661e2ec1a63c0671d3..b1bbf3ea7b68f06d4c30b0f690cea66c0014efa8 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.h
+++ b/source/armarx/navigation/components/navigation_memory/Component.h
@@ -101,7 +101,7 @@ namespace armarx::navigation::components::navigation_memory
bool visuCostmaps = true;
bool visuHumans = true;
- float visuFrequency = 2;
+ float visuFrequency = 10;
};
LocationGraph locationGraph;
};
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index f3ae9cbc89f0d9646a61750f906fa18ac3d46bc3..2b5e1aa0303af47f799986a0a90d3911ef0c981c 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -21,12 +21,15 @@
*/
#include "Visu.h"
+#include <Eigen/src/Geometry/Translation.h>
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/components/ArViz/Client/Layer.h"
+#include "RobotAPI/components/ArViz/Client/elements/Color.h"
+#include "RobotAPI/components/ArViz/Client/elements/Robot.h"
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include "armarx/navigation/conversions/eigen.h"
@@ -176,18 +179,29 @@ namespace armarx::navigation::memory
void
visualize(const human::Humans& humans, viz::Layer& layer)
{
+
+ const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0,0, 1000});
+
ARMARX_INFO << "Visualizing " << humans.size() << " humans";
for (const auto& human : humans)
{
- viz::Cylinder cylinder(std::to_string(layer.size()));
- cylinder.fromTo(conv::to3D(human.pose.translation()),
- conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
+ // viz::Cylinder cylinder(std::to_string(layer.size()));
+ // cylinder.fromTo(conv::to3D(human.pose.translation()),
+ // conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
+
+
+ // cylinder.color(simox::Color::orange());
+ // cylinder.radius(300);
+ // layer.add(cylinder);
- cylinder.color(simox::Color::orange());
- cylinder.radius(300);
+ viz::Robot mmm(std::to_string(layer.size()));
+ mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
+ mmm.pose(conv::to3D(human.pose) * human_T_mmm);
+ mmm.scale(1.7); // 1.7m
+ mmm.overrideColor(viz::Color::orange(255, 100));
+ layer.add(mmm);
- layer.add(cylinder);
}
}
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.h b/source/armarx/navigation/components/navigation_memory/Visu.h
index 123a865dbfcc38db134f9b822f502a51d02152fe..71c057df9197b918d318d738b03d86986d895464 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.h
+++ b/source/armarx/navigation/components/navigation_memory/Visu.h
@@ -25,6 +25,7 @@
#include <memory>
#include <vector>
+#include "RobotAPI/components/ArViz/Client/ScopedClient.h"
#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
@@ -57,8 +58,7 @@ namespace armarx::navigation::memory
void drawHumans(std::vector<viz::Layer>& layers, bool enabled);
- public:
- viz::Client arviz;
+ viz::ScopedClient arviz;
const armem::server::wm::CoreSegment& locSegment;
const armem::server::wm::CoreSegment& graphSegment;
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 34d27ae7372d410a97cdab175d67543b0f4ad074..3509d2f331fd0408d1cbe57d47c6a6aaa094f778 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -258,7 +258,7 @@ namespace armarx::navigation::components::navigator
fac::NavigationStackFactory::create(stackConfig, sceneProvider->scene());
// set visualization of LocalPlanner
- stack.localPlanner->setVisualization(&getArvizClient());
+ stack.localPlanner->setVisualization(getArvizClient());
memoryIntrospectors.emplace_back(
std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
diff --git a/source/armarx/navigation/core/aron/Events.xml b/source/armarx/navigation/core/aron/Events.xml
index ca7c860321674c81df923efdda4d1858fcc1d0f4..20d53656d839c54901b1a2bee51b4bfd5e5e51c3 100644
--- a/source/armarx/navigation/core/aron/Events.xml
+++ b/source/armarx/navigation/core/aron/Events.xml
@@ -46,6 +46,12 @@
</ObjectChild>
</Object>
+ <Object name='armarx::navigation::core::arondto::LocalPlanningFailedEvent'>
+ <ObjectChild key='message'>
+ <String />
+ </ObjectChild>
+ </Object>
+
<Object name='armarx::navigation::core::arondto::SafetyThrottlingTriggeredEvent'>
<ObjectChild key='pose'>
<Pose />
diff --git a/source/armarx/navigation/core/aron_conversions.cpp b/source/armarx/navigation/core/aron_conversions.cpp
index 5da2122f79898f6b9792c5b1ac1d05afede4a1ea..2a720332f8c60a6b4dd7f73d44243f7743acc0d8 100644
--- a/source/armarx/navigation/core/aron_conversions.cpp
+++ b/source/armarx/navigation/core/aron_conversions.cpp
@@ -1,8 +1,5 @@
#include "aron_conversions.h"
-#include <range/v3/range/conversion.hpp>
-#include <range/v3/view/transform.hpp>
-
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/core/Trajectory.h>
@@ -10,6 +7,8 @@
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
#include <armarx/navigation/core/types.h>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
namespace armarx::navigation::core
{
@@ -272,4 +271,18 @@ namespace armarx::navigation::core
aron::fromAron(dto.pose, bo.pose.matrix());
}
+ void
+ fromAron(const armarx::navigation::core::arondto::LocalPlanningFailedEvent& dto,
+ armarx::navigation::core::LocalPlanningFailedEvent& bo)
+ {
+ aron::fromAron(dto.message, bo.message);
+ }
+
+ void
+ toAron(armarx::navigation::core::arondto::LocalPlanningFailedEvent& dto,
+ const armarx::navigation::core::LocalPlanningFailedEvent& bo)
+ {
+ aron::toAron(dto.message, bo.message);
+ }
+
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/aron_conversions.h b/source/armarx/navigation/core/aron_conversions.h
index 2fd54753fecaf1b34346398eb5113b2a8daf36b4..408385df56046ca7b82d9a6fe310bd8e0d0aa879 100644
--- a/source/armarx/navigation/core/aron_conversions.h
+++ b/source/armarx/navigation/core/aron_conversions.h
@@ -82,6 +82,12 @@ namespace armarx::navigation::core
void toAron(armarx::navigation::core::arondto::GlobalPlanningFailedEvent& dto,
const armarx::navigation::core::GlobalPlanningFailedEvent& bo);
+ void fromAron(const armarx::navigation::core::arondto::LocalPlanningFailedEvent& dto,
+ armarx::navigation::core::LocalPlanningFailedEvent& bo);
+
+ void toAron(armarx::navigation::core::arondto::LocalPlanningFailedEvent& dto,
+ const armarx::navigation::core::LocalPlanningFailedEvent& bo);
+
void fromAron(const armarx::navigation::core::arondto::MovementStartedEvent& dto,
armarx::navigation::core::MovementStartedEvent& bo);
diff --git a/source/armarx/navigation/core/events.h b/source/armarx/navigation/core/events.h
index 09ef7781be7fc2f55e481d199c851f89b3d2d4db..594f1301ae086fcd8d29968d8ee0f13e9fe905ab 100644
--- a/source/armarx/navigation/core/events.h
+++ b/source/armarx/navigation/core/events.h
@@ -13,6 +13,7 @@ namespace armarx::navigation::core
namespace event_names
{
inline const std::string GlobalPlanningFailed = "GlobalPlanningFailedEvent";
+ inline const std::string LocalPlanningFailed = "LocalPlanningFailedEvent";
inline const std::string MovementStarted = "MovementStartedEvent";
inline const std::string GoalReached = "GoalReachedEvent";
inline const std::string WaypointReached = "WaypointReachedEvent";
@@ -31,6 +32,11 @@ namespace armarx::navigation::core
{
std::string message;
};
+
+ struct LocalPlanningFailedEvent : public Event
+ {
+ std::string message;
+ };
struct MovementStartedEvent : Event
{
diff --git a/source/armarx/navigation/global_planning/SPFA.cpp b/source/armarx/navigation/global_planning/SPFA.cpp
index 9491705a0db21cd7d53e3ecfc037eed93b9b73d5..62b4206857ae2705797416dc152f9e3175854838 100644
--- a/source/armarx/navigation/global_planning/SPFA.cpp
+++ b/source/armarx/navigation/global_planning/SPFA.cpp
@@ -91,6 +91,9 @@ namespace armarx::navigation::global_planning
// FIXME check if costmap is available
algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams;
+
+ ARMARX_CHECK(scene.staticScene.has_value());
+ ARMARX_CHECK(scene.staticScene->distanceToObstaclesCostmap.has_value());
algorithms::spfa::ShortestPathFasterAlgorithm planner(*scene.staticScene->distanceToObstaclesCostmap,
spfaParams);
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
index 4b2182bfa7527b8e5f34f91110b699d994b42906..d7ac31282e0e59b0f710a469b609dc3489774e07 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
@@ -15,8 +15,6 @@
#include <ceres/rotation.h>
#include <ceres/sized_cost_function.h>
-#include <Eigen/src/Geometry/Rotation2D.h>
-
#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <SimoxUtility/math/periodic/periodic_clamp.h>
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.cpp b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
index e64958400d3982c2cc7c3efde91408faf1f6f0cf..8a6aac53e45052b0359a4a9a13ba98df51aa15ba 100644
--- a/source/armarx/navigation/human/ProxemicZoneCreator.cpp
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
@@ -15,14 +15,15 @@ namespace armarx::navigation::human
.weight = params.intimateWeight};
// personal zone
- auto& global_T_human = human.pose;
- auto& global_V_offset = global_T_human * params.offset;
+ auto& global_R_human = human.pose.linear();
+ auto& global_V_offset = global_R_human * params.offset;
core::Pose2D pose = human.pose;
pose.translation() += global_V_offset;
float velocityNorm = human.linearVelocity.norm();
- float movementStretch = 1 + velocityNorm * params.movementInfluence;
+ // stretch with velocity in m/s as factor
+ float movementStretch = 1 + velocityNorm * params.movementInfluence / 1000; // [mm] to [m]
if (velocityNorm != 0)
{
// y pointing forward
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.h b/source/armarx/navigation/human/ProxemicZoneCreator.h
index d1696db4de9837d77132faf9c58583334450547b..fed4c18cce4105e0ffba8a6023bcb68268ae4b41 100644
--- a/source/armarx/navigation/human/ProxemicZoneCreator.h
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.h
@@ -38,7 +38,9 @@ namespace armarx::navigation::human
float personalRadius = 1000;
float movementInfluence = 1;
- // in the coordinate system of the human
+ // an offset applied to the personal zone in the coordinate system of the human
+ // a positive x-value means an offset to the right
+ // a positive y-value means an offset to the front
Eigen::Vector2f offset{100, 150};
};
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.cpp b/source/armarx/navigation/local_planning/LocalPlanner.cpp
index 4b7ea7ed74a41cec3e7f01c31c320d77ade4a813..ab0fbd229acf07ea26833e9a77481fe923f6237e 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.cpp
+++ b/source/armarx/navigation/local_planning/LocalPlanner.cpp
@@ -7,8 +7,8 @@ namespace armarx::navigation::local_planning
}
void
- LocalPlanner::setVisualization(viz::Client* vis)
+ LocalPlanner::setVisualization(viz::Client& vis)
{
- arviz = vis;
+ arviz.emplace(vis);
}
} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.h b/source/armarx/navigation/local_planning/LocalPlanner.h
index f2f6641b622a78101a4fd5892d0170ce764c4661..031318818e623d942d7acd3469d5bf771722890f 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.h
+++ b/source/armarx/navigation/local_planning/LocalPlanner.h
@@ -58,10 +58,10 @@ namespace armarx::navigation::local_planning
virtual std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) = 0;
- void setVisualization(viz::Client* vis);
+ void setVisualization(viz::Client& vis);
protected:
- viz::Client* arviz;
+ std::optional<viz::ScopedClient> arviz;
private:
const core::Scene& context;
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index 5e46a90175f9455ac1a8ab42176f862605d991d4..8f82f74d2657f27fa47d28f5a9fac7f0ae183f60 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -29,7 +29,7 @@ namespace armarx::navigation::local_planning
auto obst = boost::make_shared<teb_local_planner::PolygonObstacle>();
const Eigen::Vector2d min = conv::toRos(bbox.getMin());
- const Eigen::Vector2d max = conv::toRos(bbox.getMin());
+ const Eigen::Vector2d max = conv::toRos(bbox.getMax());
obst->pushBackVertex(min);
obst->pushBackVertex(min.x(), max.y());
@@ -39,6 +39,7 @@ namespace armarx::navigation::local_planning
obst->finalizePolygon();
container.push_back(obst);
+ // visualize bounding box if layer is available
if (visLayer != nullptr)
{
const Eigen::Vector3f min3d = conv::fromRos(min);
@@ -58,6 +59,7 @@ namespace armarx::navigation::local_planning
{
auto proxemicZones = proxemics.createProxemicZones(human);
+ int i = 0;
for (const auto& proxemicZone : proxemicZones)
{
auto pose = conv::toRos(proxemicZone.pose);
@@ -82,16 +84,18 @@ namespace armarx::navigation::local_planning
container.push_back(obst);
+ // visualize proxemic zone if layer is available
if (visLayer != nullptr)
{
- const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 0);
- const core::Pose pose3d = conv::to3D(human.pose);
+ const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 10.f-i);
+ const core::Pose pose3d = conv::to3D(proxemicZone.pose);
visLayer->add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex++))
.pose(pose3d)
.axisLengths(axisLength)
- .color(simox::Color::blue()));
+ .color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]));
}
+ i++;
}
}
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.h b/source/armarx/navigation/local_planning/TebObstacleManager.h
index b336b65bf32f42994851bf67edd5abab3bd752aa..b8859c4bbcb8145c3e1bb159e3ba7490331b14e4 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.h
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.h
@@ -50,6 +50,9 @@ namespace armarx::navigation::local_planning
teb_local_planner::ObstContainer& container;
human::ProxemicZoneCreator proxemics;
int visualizationIndex;
+
+ const std::array<simox::Color, 2> PROXEMIC_ZONE_COLOR = {simox::Color::red(),
+ simox::Color::blue()};
};
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 2633583dafd120cf02f10839d8cc26b131b90aae..c67b6256f5f601490a51f6bd3aad0ca4d1a8aecd 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -1,4 +1,5 @@
#include "TimedElasticBands.h"
+#include <optional>
#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/json/json.hpp>
@@ -112,7 +113,7 @@ namespace armarx::navigation::local_planning
geometry_msgs::Twist velocity_start = conv::toRos(scene.platformVelocity);
- //TODO (SALt): fill obstacles
+
fillObstacles();
try
@@ -124,15 +125,22 @@ namespace armarx::navigation::local_planning
ARMARX_DEBUG << "Caugth exception while planning: " << e.what();
return {};
}
+
+ if(hcp_->getTrajectoryContainer().empty())
+ {
+ ARMARX_VERBOSE << "Did not find any trajectory!";
+ return std::nullopt;
+ }
+
ARMARX_VERBOSE << "Planned successfully (found " << hcp_->getTrajectoryContainer().size()
<< " Trajectories)";
core::LocalTrajectory best = conv::fromRos(hcp_->findBestTeb()->teb());
// visualize path alternatives
- if (arviz != nullptr)
+ if (arviz)
{
- auto layer = arviz->layer("local_planner_path_alternatives");
+ auto layer = arviz.value().layer("local_planner_path_alternatives");
int i = 0;
int bestTebIdx = hcp_->bestTebIdx();
@@ -152,7 +160,8 @@ namespace armarx::navigation::local_planning
.color(simox::Color::gray()));
i++;
}
- arviz->commit(layer);
+
+ arviz.value().commit(layer);
}
return {{.trajectory = best}};
@@ -165,9 +174,9 @@ namespace armarx::navigation::local_planning
viz::Layer* visPtr = nullptr;
viz::Layer visLayer;
- if (arviz != nullptr)
+ if (arviz)
{
- visLayer = arviz->layer("local_planner_obstacles");
+ visLayer = arviz.value().layer("local_planner_obstacles");
visPtr = &visLayer;
}
@@ -189,9 +198,9 @@ namespace armarx::navigation::local_planning
}
}
- if (arviz != nullptr)
+ if (arviz)
{
- arviz->commit(visLayer);
+ arviz.value().commit(visLayer);
}
ARMARX_INFO << "TEB: added " << obstManager.size() << " obstacles";
diff --git a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
index 352c92130783063418c0b6d59ec15080d3209491..9d0873c3669327ef28f03d3b05cdd4dbffa1f1d5 100644
--- a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
+++ b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
@@ -14,6 +14,10 @@
<bool />
</ObjectChild>
+ <ObjectChild key='weight_costmap'>
+ <float />
+ </ObjectChild>
+
<ObjectChild key='weight_global_path_position'>
<float />
</ObjectChild>
diff --git a/source/armarx/navigation/local_planning/aron_conversions.cpp b/source/armarx/navigation/local_planning/aron_conversions.cpp
index 0601914bd0e0cc286ef83d2bf3badbfdd8c6d22c..928c543e8824160ec1be439041a2c40f2189e733 100644
--- a/source/armarx/navigation/local_planning/aron_conversions.cpp
+++ b/source/armarx/navigation/local_planning/aron_conversions.cpp
@@ -29,6 +29,7 @@ namespace armarx::navigation::local_planning
bo.pse.pse_costum_obstacle_penalties = dto.teb_config.pse.pse_costum_obstacle_penalties;
bo.pse.pse_costum_obstacle_penalties_dynamic =
dto.teb_config.pse.pse_costum_obstacle_penalties_dynamic;
+ bo.pse.weight_costmap = dto.teb_config.pse.weight_costmap;
bo.pse.weight_global_path_position = dto.teb_config.pse.weight_global_path_position;
bo.pse.weight_global_path_orientation = dto.teb_config.pse.weight_global_path_orientation;
bo.pse.lrk_use_alternative_approach = dto.teb_config.pse.lrk_use_alternative_approach;
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
index 4bb21ef4249d50d7965c5fd1134844968cc1f491..86fd6a2c954ab6a9d44a44e7255eea850b2d1711 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/source/armarx/navigation/local_planning/ros_conversions.cpp
@@ -49,6 +49,8 @@ namespace armarx::navigation::conv
// we change it such that x is pointing forward.
// ROS: x pointing forward, ArmarX: y pointing forward
theta += M_PI_2;
+ // normalize angle
+ theta = g2o::normalize_theta(theta);
return {pos / 1000., theta}; // [mm] to [m]
}
diff --git a/source/armarx/navigation/memory/client/events/Writer.cpp b/source/armarx/navigation/memory/client/events/Writer.cpp
index 83009655f02b687e2a00763a0e275a5f56bd8e17..ebfb044913e1c31c85b222f6149ae3b4abcd5917 100644
--- a/source/armarx/navigation/memory/client/events/Writer.cpp
+++ b/source/armarx/navigation/memory/client/events/Writer.cpp
@@ -89,6 +89,14 @@ namespace armarx::navigation::memory::client::events
event, core::event_names::GlobalPlanningFailed, clientID);
}
+ bool
+ Writer::store(const core::LocalPlanningFailedEvent& event, const std::string& clientID)
+ {
+ return storeImpl<core::arondto::LocalPlanningFailedEvent>(
+ event, core::event_names::LocalPlanningFailed, clientID);
+ }
+
+
bool
Writer::store(const core::MovementStartedEvent& event, const std::string& clientID)
{
diff --git a/source/armarx/navigation/memory/client/events/Writer.h b/source/armarx/navigation/memory/client/events/Writer.h
index 7ad07d7d4ebc2cbe051c59b8b7dedb5d5d158e98..ddbfad497d3fb6d9444b31327ff36593c48c1fe7 100644
--- a/source/armarx/navigation/memory/client/events/Writer.h
+++ b/source/armarx/navigation/memory/client/events/Writer.h
@@ -42,6 +42,7 @@ namespace armarx::navigation::memory::client::events
bool store(const core::InternalErrorEvent& event, const std::string& clientID);
bool store(const core::UserAbortTriggeredEvent& event, const std::string& clientID);
bool store(const core::GlobalPlanningFailedEvent& event, const std::string& clientID);
+ bool store(const core::LocalPlanningFailedEvent& event, const std::string& clientID);
bool store(const core::MovementStartedEvent& event, const std::string& clientID);
bool store(const core::SafetyThrottlingTriggeredEvent& event, const std::string& clientID);
bool store(const core::SafetyStopTriggeredEvent& event, const std::string& clientID);
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 6c4c51b90884528f63a4239ae9fc162ba0d10b70..b28f8886d56d4baac4a649c7502ff99e0345d35f 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -2,6 +2,7 @@
#include <algorithm>
#include <chrono>
+#include <cstddef>
#include <optional>
#include <thread>
@@ -805,9 +806,10 @@ namespace armarx::navigation::server
armarx::core::time::StopWatch::measure([&]() { updateScene(); });
ARMARX_DEBUG << deactivateSpam(0.2)
- << "Scene update: " << duration.toMilliSecondsDouble() << "ms.";
+ << "Scene update: " << duration.toMilliSecondsDouble() << "ms.";
- srv.debugObserverHelper->setDebugObserverDatafield("scene update [ms]", duration.toMilliSecondsDouble());
+ srv.debugObserverHelper->setDebugObserverDatafield("scene update [ms]",
+ duration.toMilliSecondsDouble());
}
// global planner update if goal has changed
@@ -882,9 +884,10 @@ namespace armarx::navigation::server
}
});
ARMARX_DEBUG << deactivateSpam(0.2)
- << "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
+ << "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
- srv.debugObserverHelper->setDebugObserverDatafield("local planner update [ms]", duration.toMilliSecondsDouble());
+ srv.debugObserverHelper->setDebugObserverDatafield("local planner update [ms]",
+ duration.toMilliSecondsDouble());
}
// monitor update
@@ -895,9 +898,10 @@ namespace armarx::navigation::server
armarx::core::time::StopWatch::measure([&]() { updateMonitor(); });
ARMARX_DEBUG << deactivateSpam(0.2)
- << "Monitor update: " << duration.toMilliSecondsDouble() << "ms.";
+ << "Monitor update: " << duration.toMilliSecondsDouble() << "ms.";
- srv.debugObserverHelper->setDebugObserverDatafield("monitor update [ms]", duration.toMilliSecondsDouble());
+ srv.debugObserverHelper->setDebugObserverDatafield("monitor update [ms]",
+ duration.toMilliSecondsDouble());
}
}
@@ -913,18 +917,28 @@ namespace armarx::navigation::server
{
ARMARX_CHECK(hasLocalPlanner());
- localPlan = config.stack.localPlanner->plan(globalPlan->trajectory);
-
- if (localPlan.has_value())
+ try
{
- srv.publisher->localTrajectoryUpdated(localPlan.value());
+ localPlan = config.stack.localPlanner->plan(globalPlan->trajectory);
+ if (localPlan.has_value())
+ {
+ srv.publisher->localTrajectoryUpdated(localPlan.value());
+ }
+ return localPlan;
}
- else
+ catch (...)
{
- // srv.publisher->localTrajectoryPlanningFailed();
+ ARMARX_WARNING << "Failure in local planner: " << GetHandledExceptionString();
+ srv.publisher->localPlanningFailed(core::LocalPlanningFailedEvent{
+ {.timestamp = armarx::Clock::Now()}, {GetHandledExceptionString()}});
+
+ return std::nullopt;
}
- return localPlan;
+ srv.publisher->localPlanningFailed(core::LocalPlanningFailedEvent{
+ {.timestamp = armarx::Clock::Now()}, {"Unknown reason"}});
+
+ return std::nullopt;
}
void
@@ -947,6 +961,7 @@ namespace armarx::navigation::server
{
ARMARX_INFO << "Local plan is invalid!";
srv.executor->stop();
+ return;
}
diff --git a/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h b/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
index 6ead929f13ccbdd76b9b539f2600d404aab27830..c9cf86ac8e6d89fdd0da20fd5b7ab6f600524207 100644
--- a/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
+++ b/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
@@ -24,6 +24,7 @@ namespace armarx::navigation::server
// trajectoryControllerUpdated(const traj_ctrl::local::TrajectoryControllerResult& res) = 0;
virtual void globalPlanningFailed(const core::GlobalPlanningFailedEvent& event) = 0;
+ virtual void localPlanningFailed(const core::LocalPlanningFailedEvent& event) = 0;
virtual void movementStarted(const core::MovementStartedEvent& event) = 0;
diff --git a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
index 36f4e960bd09566e23a9c69c609db02fe7cc5072..0ea9699c8de869472709d8ab6d3a63b0da979118 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
@@ -70,6 +70,11 @@ namespace armarx::navigation::server
{
eventsWriter->store(event, clientId);
}
+
+ void MemoryPublisher::localPlanningFailed(const core::LocalPlanningFailedEvent& event)
+ {
+ eventsWriter->store(event, clientId);
+ }
void
MemoryPublisher::movementStarted(const core::MovementStartedEvent& event)
diff --git a/source/armarx/navigation/server/event_publishing/MemoryPublisher.h b/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
index 831510f49b0cd8d4ab16f6f60723be07c071835d..edf5ff0d3a7908657821ed4a465ce268f81af08c 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
@@ -22,6 +22,8 @@ namespace armarx::navigation::server
// const traj_ctrl::local::TrajectoryControllerResult& res) 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;
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
index ef5077438bd3c6d46dd7be64d8e72b0cbb99cffe..e7bb44d8c57df8952fd317e446f6e8cbab23284c 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -80,6 +80,11 @@ namespace armarx::navigation::server
// sends the updated config to the controller and stores it in the memory
ctrl->updateConfig();
+
+ if(not ctrl->ctrl()->isControllerActive())
+ {
+ start();
+ }
}
void
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index d4efe2e122112d6d077d9599e758deb857ccfbd6..6ff9fe140c7c09277aae20d9dbc96369d390f6b0 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -6,6 +6,7 @@
#include <Eigen/Geometry>
#include <SimoxUtility/algorithm/apply.hpp>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/ColorMap.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
@@ -45,7 +46,7 @@ namespace armarx::navigation::server
ARMARX_DEBUG << "ArvizIntrospector::onGlobalPlannerResult";
drawGlobalTrajectory(result.trajectory);
- arviz.commit(layers);
+ arviz.commit(simox::alg::get_values(layers));
visualization.visualize(result.trajectory);
@@ -55,7 +56,7 @@ namespace armarx::navigation::server
ArvizIntrospector::onLocalPlannerResult(const local_planning::LocalPlannerResult& result)
{
drawLocalTrajectory(result.trajectory);
- arviz.commit(layers);
+ arviz.commit(simox::alg::get_values(layers));
}
// void
@@ -84,7 +85,7 @@ namespace armarx::navigation::server
visualization.setTarget(goal);
}
-
+
void
ArvizIntrospector::onGlobalShortestPath(const std::vector<core::Pose>& path)
{
@@ -143,7 +144,7 @@ namespace armarx::navigation::server
.color(cmap.at(tp.velocity / maxVelocity)));
}
- layers.emplace_back(std::move(layer));
+ layers[layer.data_.name] = std::move(layer);
}
void
@@ -182,8 +183,8 @@ namespace armarx::navigation::server
viz::Sphere("velocity_" + std::to_string(i)).position(pos).radius(50).color(color));
}
- layers.emplace_back(std::move(layer));
- layers.emplace_back(std::move(velLayer));
+ layers[layer.data_.name] = std::move(layer);
+ layers[velLayer.data_.name] = std::move(velLayer);
}
void
@@ -196,7 +197,7 @@ namespace armarx::navigation::server
core::Pose(robot->getGlobalPose()) * twist.linear)
.color(simox::Color::orange()));
- layers.emplace_back(std::move(layer));
+ layers[layer.data_.name] = std::move(layer);
}
void
@@ -209,7 +210,7 @@ namespace armarx::navigation::server
core::Pose(robot->getGlobalPose()) * twist.linear)
.color(simox::Color::green()));
- layers.emplace_back(std::move(layer));
+ layers[layer.data_.name] = std::move(layer);
}
ArvizIntrospector::ArvizIntrospector(ArvizIntrospector&& other) noexcept :
@@ -220,6 +221,22 @@ namespace armarx::navigation::server
{
}
+ void ArvizIntrospector::clear()
+ {
+ // clear all layers
+ for(auto& [name, layer]: layers)
+ {
+ layer.markForDeletion();
+ }
+ arviz.commit(simox::alg::get_values(layers));
+ layers.clear();
+
+ // some special internal layers of TEB
+ arviz.commitDeleteLayer("local_planner_obstacles");
+ arviz.commitDeleteLayer("local_planner_velocity");
+ arviz.commitDeleteLayer("local_planner_path_alternatives");
+ }
+
ArvizIntrospector&
ArvizIntrospector::operator=(ArvizIntrospector&&) noexcept
{
@@ -262,11 +279,15 @@ namespace armarx::navigation::server
void
ArvizIntrospector::success()
{
+ clear();
+
visualization.success();
}
void
ArvizIntrospector::failure()
{
+ clear();
+
visualization.failed();
}
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index c784e0de09fc82a1cc1e8094a2549914be0748df..a0a5bfc1e2e9e2949fdf01f5ca0c79228c5224db 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -69,6 +69,8 @@ namespace armarx::navigation::server
ArvizIntrospector(ArvizIntrospector&& other) noexcept;
ArvizIntrospector& operator=(ArvizIntrospector&&) noexcept;
+ void clear();
+
private:
void drawGlobalTrajectory(const core::GlobalTrajectory& trajectory);
void drawLocalTrajectory(const core::LocalTrajectory& trajectory);
@@ -78,7 +80,7 @@ namespace armarx::navigation::server
viz::ScopedClient arviz;
const VirtualRobot::RobotPtr robot;
- std::vector<viz::Layer> layers;
+ std::map<std::string, viz::Layer> layers;
util::Visualization visualization;
};
diff --git a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h
index 34f9e49d72d2a69b3430a15397b5b09c02cb357b..ddfdc0bc96ef6b193045722c1534287f007aea07 100644
--- a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h
+++ b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h
@@ -34,7 +34,7 @@ namespace armarx::navigation::server
struct GoalReachedMonitorConfig
{
float posTh{70.F}; // [mm]
- float oriTh{VirtualRobot::MathTools::deg2rad(5.F)}; // [rad]
+ float oriTh{VirtualRobot::MathTools::deg2rad(3.F)}; // [rad]
float linearVelTh{100.F}; // [mm/s]
float angularVelTh{VirtualRobot::MathTools::deg2rad(5.F)}; // [rad/s]
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
index 26d9cd41fbe72fc72082b7d9ffc5d08e06e78d02..d9977c6e529476977eb0c37e5f4f0b4c682b6ab6 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
@@ -84,10 +84,6 @@ namespace armarx::navigation::server::scene_provider
ARMARX_INFO << objects->getSize() << " objects in the scene";
ARMARX_INFO << "Retrieving costmap in memory";
- const memory::client::costmap::Reader::Query query{.providerName =
- config.staticCostmapProviderName,
- .name = config.staticCostmapName,
- .timestamp = armarx::Clock::Now()};
ARMARX_TRACE;
@@ -96,10 +92,17 @@ namespace armarx::navigation::server::scene_provider
// waiting for static costmap to become available
while (true)
{
+ const memory::client::costmap::Reader::Query query{
+ .providerName = config.staticCostmapProviderName,
+ .name = config.staticCostmapName,
+ .timestamp = armarx::Clock::Now()};
+
+
if (const memory::client::costmap::Reader::Result costmap =
srv.costmapReader->query(query))
{
ARMARX_CHECK(costmap.costmap.has_value());
+ ARMARX_INFO << "Static costmap available.";
return costmap.costmap.value();
}
diff --git a/source/armarx/navigation/simulation/CMakeLists.txt b/source/armarx/navigation/simulation/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..774fdf64c64b88f386093d8f0b961db534d11c5e
--- /dev/null
+++ b/source/armarx/navigation/simulation/CMakeLists.txt
@@ -0,0 +1,23 @@
+armarx_add_library(simulation
+ SOURCES
+ simulation.cpp
+ SimulatedHuman.cpp
+ HEADERS
+ simulation.h
+ SimulatedHuman.h
+ DEPENDENCIES_PUBLIC
+ ArmarXCoreInterfaces
+ ArmarXCore
+ # armarx_navigation
+ armarx_navigation::core
+ armarx_navigation::conversions
+ armarx_navigation::global_planning
+)
+
+
+armarx_add_test(simulationTest
+ TEST_FILES
+ test/simulationTest.cpp
+ DEPENDENCIES
+ navigation::simulation
+)
diff --git a/source/armarx/navigation/simulation/SimulatedHuman.cpp b/source/armarx/navigation/simulation/SimulatedHuman.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a4d735dff8efc31ca6e49c987568af159139b24
--- /dev/null
+++ b/source/armarx/navigation/simulation/SimulatedHuman.cpp
@@ -0,0 +1,152 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "SimulatedHuman.h"
+
+#include <VirtualRobot/Random.h>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/algorithms/util.h"
+#include "armarx/navigation/conversions/eigen.h"
+#include "armarx/navigation/core/StaticScene.h"
+#include "armarx/navigation/core/types.h"
+#include "armarx/navigation/global_planning/SPFA.h"
+#include "armarx/navigation/human/types.h"
+
+namespace armarx::navigation::human::simulation
+{
+
+
+ human::Human
+ SimulatedHuman::update()
+ {
+ switch (state_)
+ {
+ case State::Idle:
+ ARMARX_DEBUG << "State:Idle";
+ initialize();
+ state_ = State::Walking;
+ break;
+ case State::Walking:
+ {
+ ARMARX_DEBUG << "State:Walking";
+
+ step();
+
+ const Eigen::Vector2f goal =
+ conv::to2D(globalTrajectory_.points().back().waypoint.pose.translation());
+
+ if ((goal - human_.pose.translation()).norm() < params_.goalDistanceThreshold)
+ {
+ ARMARX_INFO << "Human reached goal";
+ state_ = State::GoalReached;
+ }
+
+ break;
+ }
+ case State::GoalReached:
+ ARMARX_DEBUG << "State:GoalReached";
+
+ // TODO(fabian.reister): simulate "perform action at goal": wait a while until movement is started again.
+ state_ = State::Idle;
+ break;
+ }
+
+ return human_;
+ }
+
+ void
+ SimulatedHuman::initialize()
+ {
+ global_planning::SPFA::Params params;
+ params.linearVelocity = this->params_.maxLinearVelocity;
+
+ core::Scene scene;
+
+ scene.staticScene.emplace(core::StaticScene{nullptr, std::nullopt});
+ scene.staticScene->distanceToObstaclesCostmap.emplace(distanceField_);
+
+ global_planning::SPFA planner(params, scene);
+
+ const auto start = human_.pose;
+
+ while (true)
+ {
+ const auto sampledPose = algorithms::sampleValidPositionInMap(distanceField_);
+ ARMARX_CHECK(sampledPose.has_value());
+ const auto& goal = sampledPose.value();
+
+ const auto plan = planner.plan(conv::to3D(start), conv::to3D(goal));
+
+ // check if plan could be created. otherwise try another goal
+ if (plan.has_value())
+ {
+ globalTrajectory_ = plan->trajectory;
+
+ human_ = human::Human{.pose = start,
+ .linearVelocity = Eigen::Vector2f::Zero(),
+ .detectionTime = Clock::Now()};
+ return;
+ }
+ }
+ }
+
+ void
+ SimulatedHuman::step()
+ {
+ const auto updateTime = Clock::Now();
+
+ const auto duration = updateTime - human_.detectionTime;
+
+ // move according to old state
+ human_.pose.translation() += duration.toSecondsDouble() * human_.linearVelocity;
+
+ const auto projection =
+ globalTrajectory_.getProjection(conv::to3D(human_.pose.translation()),
+ core::VelocityInterpolation::LinearInterpolation);
+
+ human_.pose = conv::to2D(projection.projection.waypoint.pose);
+
+ const auto wpBefore = projection.wayPointBefore.waypoint.pose.translation();
+ const auto wpAfter = projection.wayPointAfter.waypoint.pose.translation();
+
+ human_.linearVelocity =
+ (wpAfter - wpBefore).head<2>().normalized() * projection.projection.velocity;
+
+ human_.linearVelocity =
+ human_.linearVelocity.normalized() * std::clamp(human_.linearVelocity.norm(),
+ params_.minLinearVelocity,
+ params_.maxLinearVelocity);
+
+ human_.detectionTime = updateTime;
+ }
+
+
+ SimulatedHuman::SimulatedHuman(const algorithms::Costmap& distanceField, const Params& params) :
+ distanceField_(distanceField), params_(params)
+ {
+ const auto sampledPose = algorithms::sampleValidPositionInMap(distanceField_);
+ ARMARX_CHECK(sampledPose.has_value());
+ human_.pose = sampledPose.value();
+ }
+} // namespace armarx::navigation::human::simulation
diff --git a/source/armarx/navigation/simulation/SimulatedHuman.h b/source/armarx/navigation/simulation/SimulatedHuman.h
new file mode 100644
index 0000000000000000000000000000000000000000..fa4ed6a8a01a748937c8aa4a57b5f9185705cbe7
--- /dev/null
+++ b/source/armarx/navigation/simulation/SimulatedHuman.h
@@ -0,0 +1,73 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/core/Trajectory.h"
+#include "armarx/navigation/core/basic_types.h"
+#include "armarx/navigation/human/types.h"
+
+namespace armarx::navigation::human::simulation
+{
+
+ struct SimulatedHumanParams
+ {
+ float goalDistanceThreshold = 100;
+
+ float minLinearVelocity = 100;
+ float maxLinearVelocity = 200;
+ };
+
+ class SimulatedHuman
+ {
+ public:
+ using Params = SimulatedHumanParams;
+
+ SimulatedHuman(const algorithms::Costmap& distanceField, const Params& params = Params());
+
+ Human update();
+
+ protected:
+ void initialize();
+
+ void step();
+
+ enum class State
+ {
+ Idle,
+ Walking,
+ GoalReached
+ };
+
+ private:
+ const algorithms::Costmap distanceField_;
+
+ State state_ = State::Idle;
+
+ human::Human human_;
+
+ core::GlobalTrajectory globalTrajectory_;
+
+ const Params params_;
+ };
+
+} // namespace armarx::navigation::human::simulation
diff --git a/source/armarx/navigation/simulation/simulation.cpp b/source/armarx/navigation/simulation/simulation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fee4cfba9429e62bb62a82d2eb761fd78fa36d3c
--- /dev/null
+++ b/source/armarx/navigation/simulation/simulation.cpp
@@ -0,0 +1,28 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package navigation::ArmarXObjects::simulation
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "simulation.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/armarx/navigation/simulation/simulation.h b/source/armarx/navigation/simulation/simulation.h
new file mode 100644
index 0000000000000000000000000000000000000000..36a6c6b3060d34a31cd880dc0e090ee2e22149eb
--- /dev/null
+++ b/source/armarx/navigation/simulation/simulation.h
@@ -0,0 +1,45 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package navigation::ArmarXObjects::simulation
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-simulation simulation
+ * @ingroup navigation
+ * A description of the library simulation.
+ *
+ * @class simulation
+ * @ingroup Library-simulation
+ * @brief Brief description of class simulation.
+ *
+ * Detailed description of class simulation.
+ */
+ class simulation
+ {
+ public:
+
+ };
+
+}
diff --git a/source/armarx/navigation/simulation/test/simulationTest.cpp b/source/armarx/navigation/simulation/test/simulationTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eb077826955bedc0478271ab6f7e97d2d2c3699c
--- /dev/null
+++ b/source/armarx/navigation/simulation/test/simulationTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package navigation::ArmarXObjects::simulation
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE navigation::ArmarXLibraries::simulation
+
+#define ARMARX_BOOST_TEST
+
+#include <navigation/Test.h>
+#include "../simulation.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+ BOOST_CHECK_EQUAL(true, true);
+}