diff --git a/CMakeLists.txt b/CMakeLists.txt
index 49d3cadaf1823b11fe8d2c9b14a641cea2826788..fc6e0627c607299fc0eaa0b6a8f6a1103cc28dd8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,26 +12,39 @@ 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)
+armarx_find_package(PUBLIC HRVO QUIET)
+armarx_find_package(PUBLIC RVO QUIET)
+
+# human aware navigation
+armarx_find_package(PUBLIC teb_local_planner QUIET)
+armarx_find_package(PUBLIC teb_extension QUIET)
add_subdirectory(etc)
@@ -42,12 +55,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/build/.gitkeep b/build/.gitkeep
deleted file mode 100644
index c9f1420cf14cd0e68c67826372e872bf0c7a29d6..0000000000000000000000000000000000000000
--- a/build/.gitkeep
+++ /dev/null
@@ -1 +0,0 @@
-# This file is tracked to keep empty directories in Git
diff --git a/data/armarx_navigation/controller_config/GlobalTrajectory/default.json b/data/armarx_navigation/controller_config/GlobalTrajectory/default.json
new file mode 100644
index 0000000000000000000000000000000000000000..431861916bf6344e810b7bd4a1b9561d4d0226fb
--- /dev/null
+++ b/data/armarx_navigation/controller_config/GlobalTrajectory/default.json
@@ -0,0 +1,23 @@
+{
+ "params": {
+ "pidPos": {
+ "Kp": 15,
+ "Ki": 0,
+ "Kd": 0
+ },
+ "pidOri": {
+ "Kp": 10,
+ "Ki": 0,
+ "Kd": 0
+ },
+ "limits": {
+ "linear": 500,
+ "angular": 0.4
+ }
+ },
+ "targets": {
+ "trajectory": {
+ "points": []
+ }
+ }
+}
diff --git a/data/armarx_navigation/controller_config/PlatformTrajectory/default.json b/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
index 4cd38813e9ad112a708650aeb00912e426037425..e69bb8299d15eab5f3de0eb5219308b88f9cd51c 100644
--- a/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
+++ b/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
@@ -1,18 +1,18 @@
{
"params": {
"pidPos": {
- "Kp": 1,
+ "Kp": 0.1,
"Ki": 0,
"Kd": 0
},
"pidOri": {
- "Kp": 1,
+ "Kp": 0.1,
"Ki": 0,
"Kd": 0
},
"limits": {
- "linear": 300,
- "angular": 0.2
+ "linear": 500,
+ "angular": 0.5
}
},
"targets": {
diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
new file mode 100644
index 0000000000000000000000000000000000000000..67fe56d7d947b1dc69af3c3610fef86db1aaae17
--- /dev/null
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -0,0 +1,117 @@
+{
+ "robot_footprint_radius": 0.5,
+ "planning_distance": 3,
+
+ "teb_config": {
+ "pse": {
+ "pse_costum_obstacle_penalties": true,
+ "pse_costum_obstacle_penalties_dynamic": true,
+ "weight_costmap": 1,
+ "weight_global_path_position": 0.3,
+ "weight_global_path_orientation": 0.3,
+ "lrk_use_alternative_approach": false,
+ "lrk_enable_collision_check": true,
+ "hybrid_homotopy_calculation": true,
+ "robot_diff_circumscribed_inscribed_radius": 0.0
+ },
+
+ "trajectory": {
+ "teb_autosize": true,
+ "dt_ref": 0.3,
+ "dt_hysteresis": 0.1,
+ "min_samples": 3,
+ "max_samples": 500,
+ "global_plan_overwrite_orientation": true,
+ "allow_init_with_backwards_motion": false,
+ "exact_arc_length": false,
+ "force_reinit_new_goal_dist": 1,
+ "force_reinit_new_goal_angular": 1.5707963267948966,
+ "feasibility_check_no_poses": 5,
+ "feasibility_check_lookahead_distance": -1,
+ "min_resolution_collision_check_angular": 3.141592653589793
+ },
+
+ "robot": {
+ "max_vel_x": 0.5,
+ "max_vel_x_backwards": 0.5,
+ "max_vel_y": 0.5,
+ "max_vel_trans": 0.5,
+ "max_vel_theta": 0.5,
+ "acc_lim_x": 0.5,
+ "acc_lim_y": 0.5,
+ "acc_lim_theta": 0.5,
+ "min_turning_radius": 0
+ },
+
+ "goal_tolerance": {
+ "xy_goal_tolerance": 0.0
+ },
+
+ "obstacles": {
+ "min_obstacle_dist": 0.3,
+ "inflation_dist": 0.0,
+ "dynamic_obstacle_inflation_dist": 0.6,
+ "include_dynamic_obstacles": true,
+ "obstacle_association_force_inclusion_factor": 1.5,
+ "obstacle_association_cutoff_factor": 5,
+ "obstacle_proximity_ratio_max_vel": 1,
+ "obstacle_proximity_lower_bound": 0,
+ "obstacle_proximity_upper_bound": 0.5
+ },
+
+ "optim": {
+ "no_inner_iterations": 5,
+ "no_outer_iterations": 4,
+ "optimization_activate": true,
+ "optimization_verbose": false,
+ "penalty_epsilon": 0.05,
+ "weight_max_vel_x": 2,
+ "weight_max_vel_y": 2,
+ "weight_max_vel_theta": 1,
+ "weight_acc_lim_x": 1,
+ "weight_acc_lim_y": 1,
+ "weight_acc_lim_theta": 1,
+ "weight_kinematics_nh": 1,
+ "weight_kinematics_forward_drive": 1,
+ "weight_kinematics_turning_radius": 1,
+ "weight_optimaltime": 1,
+ "weight_shortest_path": 1,
+ "weight_obstacle": 50,
+ "weight_inflation": 0.1,
+ "weight_dynamic_obstacle": 50,
+ "weight_dynamic_obstacle_inflation": 0.1,
+ "weight_velocity_obstacle_ratio": 0,
+ "weight_viapoint": 1,
+ "weight_prefer_rotdir": 50,
+ "weight_adapt_factor": 2.0,
+ "obstacle_cost_exponent": 1.0
+ },
+
+ "hcp": {
+ "enable_multithreading": true,
+ "simple_exploration": false,
+ "max_number_classes": 5,
+ "max_number_plans_in_current_class": 0,
+ "selection_cost_hysteresis": 1.0,
+ "selection_obst_cost_scale": 100.0,
+ "selection_viapoint_cost_scale": 1.0,
+ "selection_alternative_time_cost": false,
+ "selection_dropping_probability": 0.0,
+ "switching_blocking_period": 0.0,
+
+ "obstacle_heading_threshold": 0.45,
+ "roadmap_graph_no_samples": 20,
+ "roadmap_graph_area_width": 10,
+ "roadmap_graph_area_length_scale": 1.0,
+ "h_signature_prescaler": 1,
+ "h_signature_threshold": 0.1,
+
+ "viapoints_all_candidates": true,
+
+ "delete_detours_backwards": true,
+ "detours_orientation_tolerance": 1.5707963267948966,
+ "length_start_orientation_vector": 0.4,
+ "max_ratio_detours_duration_best_duration": 3.0
+ }
+ }
+}
diff --git a/etc/cmake/Usearmarx_navigation.cmake b/etc/cmake/Usearmarx_navigation.cmake
deleted file mode 100644
index 4c5c677a83d94e372ec21584523b68c8933b0519..0000000000000000000000000000000000000000
--- a/etc/cmake/Usearmarx_navigation.cmake
+++ /dev/null
@@ -1 +0,0 @@
-# This file contains macros for projects depending on Navigation
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 2900174136483fd185ccd334445783b143c7fef5..1110c342da3693c7868daa14a741bca2e0f67711 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -27,20 +27,22 @@
#include <cmath>
#include <thread>
-#include <Eigen/src/Geometry/AngleAxis.h>
-#include <Eigen/src/Geometry/Translation.h>
-
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/ClockType.h>
#include <ArmarXCore/core/time/forward_declarations.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
-#include <armarx/navigation/client/types.h>
-#include <armarx/navigation/global_planning/Point2Point.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/TimedElasticBandsParams.h"
#include <armarx/navigation/client/PathBuilder.h>
+#include <armarx/navigation/client/types.h>
#include <armarx/navigation/global_planning/AStar.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/global_planning/Point2Point.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
namespace armarx::navigation::components::example_client
@@ -49,13 +51,11 @@ namespace armarx::navigation::components::example_client
Component::Component()
{
addPlugin(virtualRobotReaderPlugin);
+ addPlugin(costmapReaderPlugin);
}
- Component::~Component()
- {
- // pass
- }
+ Component::~Component() = default;
void
@@ -68,7 +68,7 @@ namespace armarx::navigation::components::example_client
void
Component::onConnectComponent()
{
- task = new armarx::RunningTask<Component>(this, &Component::exampleNavigationPointToPoint);
+ task = new armarx::RunningTask<Component>(this, &Component::run);
task->start();
}
@@ -91,10 +91,91 @@ namespace armarx::navigation::components::example_client
std::string
Component::getDefaultName() const
{
- return "ExampleClient";
+ return GetDefaultName();
+ }
+
+
+ std::string
+ Component::GetDefaultName()
+ {
+ return "example_client";
}
+ void
+ Component::run()
+ {
+ switch (properties.mode)
+ {
+
+ case Mode::Standard:
+ ARMARX_IMPORTANT << "Running `Mode::Standard`";
+ exampleNavigation();
+ break;
+ case Mode::Complex:
+ ARMARX_IMPORTANT << "Running `Mode::Complex`";
+ exampleNavigationComplex();
+ break;
+ case Mode::PointToPoint:
+ ARMARX_IMPORTANT << "Running `Mode::PointToPoint`";
+ exampleNavigationPointToPoint();
+ break;
+ case Mode::UpdateNavigator:
+ ARMARX_IMPORTANT << "Running `Mode::UpdateNavigator`";
+ exampleNavigationUpdateNavigator();
+ break;
+ case Mode::WanderAround:
+ ARMARX_IMPORTANT << "Running `Mode::WanderAround`";
+ exampleNavigationWanderAround();
+ break;
+ }
+ }
+
+ void
+ Component::exampleNavigationUpdateNavigator()
+ {
+ // 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()));
+
+ // Example of registering a lambda as callback.
+ getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached!"; });
+
+ // Start moving to goal position using above config.
+
+ ARMARX_INFO << "Moving to goal pose";
+ core::Pose goal = core::Pose::Identity();
+ goal.translation() << 2000, 1000, 0;
+ getNavigator().moveTo({goal}, core::NavigationFrame::Absolute);
+ Clock::WaitFor(Duration::Seconds(1.0));
+
+
+ // after short period of time (before goal is reached), change goal
+ const std::vector<core::Pose> goals{
+ core::Pose(Eigen::Translation3f{1000, 2000, 0}),
+ core::Pose(Eigen::Translation3f{3000, 0, 0}),
+ core::Pose(Eigen::Translation3f{5000, 1000, 0}),
+ };
+
+ while (true)
+ {
+ for (const auto& nextGoal : goals)
+ {
+ Clock::WaitFor(Duration::Seconds(1.0));
+ getNavigator().update({nextGoal}, core::NavigationFrame::Absolute);
+ }
+ }
+ }
+
void
Component::exampleNavigation()
{
@@ -107,8 +188,9 @@ namespace armarx::navigation::components::example_client
configureNavigator(
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
- .globalPlanner(global_planning::AStarParams())
- .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
+ .globalPlanner(global_planning::SPFAParams())
+ .trajectoryController(
+ traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
// Example of registering a lambda as callback.
getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
@@ -206,8 +288,9 @@ namespace armarx::navigation::components::example_client
configureNavigator(
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
- .globalPlanner(global_planning::AStarParams())
- .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
+ .globalPlanner(global_planning::SPFAParams())
+ .trajectoryController(
+ traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
// Example of registering a lambda as callback.
getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
@@ -283,7 +366,8 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::Point2PointParams())
- .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
+ .trajectoryController(
+ traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
Clock::WaitFor(Duration::Seconds(1));
@@ -346,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()
@@ -366,10 +536,16 @@ namespace armarx::navigation::components::example_client
"relativeMovement",
"The distance between two target poses [mm]");
+ def->optional(properties.mode, "mode", "Which example to run")
+ .map({{"standard", Mode::Standard},
+ {"complex", Mode::Complex},
+ {"point_to_point", Mode::PointToPoint},
+ {"update_navigator", Mode::UpdateNavigator},
+ {"wander_around", Mode::WanderAround}});
+
return def;
}
-
- ARMARX_DECOUPLED_REGISTER_COMPONENT(Component);
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
} // namespace armarx::navigation::components::example_client
diff --git a/examples/components/example_client/Component.h b/examples/components/example_client/Component.h
index 295d1964401c7b2025b339e3384340b9a95b37da..f8b5bb836c7cc0c2bc3890ee9170747015dafb89 100644
--- a/examples/components/example_client/Component.h
+++ b/examples/components/example_client/Component.h
@@ -26,15 +26,26 @@
#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
{
+ enum class Mode
+ {
+ Standard,
+ Complex,
+ PointToPoint,
+ UpdateNavigator,
+ WanderAround
+ };
+
/**
* @defgroup Component-ExampleClient ExampleClient
* @ingroup armarx_navigation-Components
@@ -57,6 +68,8 @@ namespace armarx::navigation::components::example_client
~Component() override;
+ static std::string GetDefaultName();
+
protected:
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -75,6 +88,8 @@ namespace armarx::navigation::components::example_client
std::string getDefaultName() const override;
+ void run();
+
void exampleNavigation();
void exampleNavigationComplex();
@@ -91,19 +106,32 @@ namespace armarx::navigation::components::example_client
*/
void exampleNavigationPointToPoint();
+ void exampleNavigationUpdateNavigator();
+
+ void exampleNavigationWanderAround();
+
private:
void goalReached();
- struct{
+ struct
+ {
std::string robotName = "Armar6";
float relativeMovement = 200; // [mm]
+
+ Mode mode = Mode::Standard;
} properties;
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
new file mode 100644
index 0000000000000000000000000000000000000000..fc614ebb2e78653d188f50289ec1a329ff1a1055
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="HumanAwareNavigation" creation="2021-07-09.11:39:42" globalConfigName="./config/global.cfg" package="armarx_navigation" deploymentType="local" nodeName="NodeMain">
+ <application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <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="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="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"/>
+ <application name="SimpleVirtualRobot" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/HumanAwareNavigation/config/HumanMemoryApp.cfg b/scenarios/HumanAwareNavigation/config/HumanMemoryApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..28099ebc83514ac57b5af9e5acba37e72a6f3846
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/HumanMemoryApp.cfg
@@ -0,0 +1,368 @@
+# ==================================================================
+# HumanMemoryApp 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.HumanMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.HumanMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.HumanMemory.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.HumanMemory.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.HumanMemory.EnableProfiling = false
+
+
+# ArmarX.HumanMemory.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.HumanMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.HumanMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ObjectName = ""
+
+
+# ArmarX.HumanMemory.face.seg.CoreMaxHistorySize: Maximal size of the FaceRecognition entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.face.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.HumanMemory.face.seg.CoreSegmentName: Name of the FaceRecognition core segment.
+# Attributes:
+# - Default: FaceRecognition
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.face.seg.CoreSegmentName = FaceRecognition
+
+
+# ArmarX.HumanMemory.ident.seg.CoreMaxHistorySize: Maximal size of the Identification entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ident.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.HumanMemory.ident.seg.CoreSegmentName: Name of the Identification core segment.
+# Attributes:
+# - Default: Identification
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ident.seg.CoreSegmentName = Identification
+
+
+# ArmarX.HumanMemory.instanceseg.CoreMaxHistorySize: Maximal size of the PersonInstance entity histories (-1 for infinite).
+# Attributes:
+# - Default: 32
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.instanceseg.CoreMaxHistorySize = 32
+
+
+# ArmarX.HumanMemory.instanceseg.CoreSegmentName: Name of the PersonInstance core segment.
+# Attributes:
+# - Default: PersonInstance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.instanceseg.CoreSegmentName = PersonInstance
+
+
+# ArmarX.HumanMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.MemoryName = Human
+
+
+# ArmarX.HumanMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.HumanMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HumanMemory.mem.ltm.enabled = false
+
+
+# ArmarX.HumanMemory.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.HumanMemory.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.HumanMemory.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.HumanMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.HumanMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.HumanMemory.pose.seg.CoreMaxHistorySize: Maximal size of the Pose entity histories (-1 for infinite).
+# Attributes:
+# - Default: 256
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.pose.seg.CoreMaxHistorySize = 256
+
+
+# ArmarX.HumanMemory.pose.seg.CoreSegmentName: Name of the Pose core segment.
+# Attributes:
+# - Default: Pose
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.pose.seg.CoreSegmentName = Pose
+
+
+# ArmarX.HumanMemory.profile.pk.load: Load profiles from prior knowledge on startup.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HumanMemory.profile.pk.load = true
+
+
+# ArmarX.HumanMemory.profile.pk.packageName: ArmarX package to load human profiles from.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.pk.packageName = PriorKnowledgeData
+
+
+# ArmarX.HumanMemory.profile.seg.CoreMaxHistorySize: Maximal size of the Profile entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.HumanMemory.profile.seg.CoreSegmentName: Name of the Profile core segment.
+# Attributes:
+# - Default: Profile
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.seg.CoreSegmentName = Profile
+
+
+# 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
+
+
diff --git a/scenarios/HumanAwareNavigation/config/MemoryNameSystem.cfg b/scenarios/HumanAwareNavigation/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..b8bc70a66ca7f32a628886ad1bf13e373f9750d3
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/MemoryNameSystem.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# MemoryNameSystem 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.MemoryNameSystem.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.MemoryNameSystem.EnableProfiling = false
+
+
+# ArmarX.MemoryNameSystem.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.MemoryNameSystem.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MemoryNameSystem.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.ObjectName = ""
+
+
+# ArmarX.MemoryNameSystem.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.RemoteGuiName = RemoteGuiProvider
+
+
+# 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
+
+
diff --git a/scenarios/HumanAwareNavigation/config/ObjectMemory.cfg b/scenarios/HumanAwareNavigation/config/ObjectMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..5b38c4fbd210639e6d97c503b9b0a62416c2ac23
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/ObjectMemory.cfg
@@ -0,0 +1,717 @@
+# ==================================================================
+# ObjectMemory 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.ObjectMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.ObjectMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ObjectMemory.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.ObjectMemory.EnableProfiling = false
+
+
+# ArmarX.ObjectMemory.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.ObjectMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ObjectName = ""
+
+
+# ArmarX.ObjectMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
+# Attributes:
+# - Default: KinematicUnitObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName = KinematicUnitObserver
+
+
+# ArmarX.ObjectMemory.mem..marker.Name: Marker Memory Name
+# Attributes:
+# - Default: Marker
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem..marker.Name = Marker
+
+
+# ArmarX.ObjectMemory.mem..marker.maxHistorySize: Maximum marker memory history size
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem..marker.maxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Object
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.MemoryName = Object
+
+
+# ArmarX.ObjectMemory.mem.attachments.CoreSegmentName: Name of the object instance core segment.
+# Attributes:
+# - Default: Attachments
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.attachments.CoreSegmentName = Attachments
+
+
+# ArmarX.ObjectMemory.mem.attachments.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.attachments.MaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName: Object class entity of the floor.
+# Attributes:
+# - Default: Building/floor-20x20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName = Building/floor-20x20
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Height: Height (z) of the floor plane.
+# Set slightly below 0 to avoid z-fighting when drawing planes on the ground.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.Height = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName: Layer to draw the floor on.
+# Attributes:
+# - Default: Floor
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName = Floor
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Show: Whether to show the floor.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.Floor.Show = true
+
+
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage = true
+
+
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage: Name of the objects package to load from.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = PriorKnowledgeData
+
+
+# ArmarX.ObjectMemory.mem.cls.seg.CoreMaxHistorySize: Maximal size of the Class entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.seg.CoreSegmentName: Name of the Class core segment.
+# Attributes:
+# - Default: Class
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.seg.CoreSegmentName = Class
+
+
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
+# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached = true
+
+
+# ArmarX.ObjectMemory.mem.inst.calibration.offset: Offset for the node to be calibrated.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# 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
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.robotNode = Neck_2_Pitch
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds: Duration after latest localization before decay starts.
+# Attributes:
+# - Default: 5
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds = 5
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds: How long to reach minimal confidence.
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds = 20
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.enabled: If true, object poses decay over time when not localized anymore.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.decay.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence: Confidence when decay starts.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence: Confidence after decay duration.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence = 0
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence: Remove objects whose confidence is lower than this value.
+# Attributes:
+# - Default: 0.100000001
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence = 0.100000001
+
+
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity: If true, check whether the head is moving and discard updates in the meantime.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity = true
+
+
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS: For how long new updates are ignored after moving the head.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS = 100
+
+
+# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity: If a head joint's velocity is higher, the head is considered moving.
+# Attributes:
+# - Default: 0.0500000007
+# - Case sensitivity: yes
+# - Required: no
+# 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:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.10_Package = PriorKnowledgeData
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.11_Directory: Directory in Package/data/Package/ containing the scene snapshots.
+# Attributes:
+# - Default: scenes
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.11_Directory = scenes
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad: Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').
+# You can also specify paths relative to 'Package/scenes/'.
+# You can also specify a ; separated list of scenes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = ""
+
+
+# ArmarX.ObjectMemory.mem.inst.seg.CoreMaxHistorySize: Maximal size of the Instance entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.ObjectMemory.mem.inst.seg.CoreSegmentName: Name of the Instance core segment.
+# Attributes:
+# - Default: Instance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.seg.CoreSegmentName = Instance
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.alpha = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence: If true, use the pose confidence as alpha (if < 1.0).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.enabled: Enable or disable visualization of objects.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.enabled = true
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz: Frequency of visualization.
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz = 25
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.position: Enable showing pose gaussians (orientation part).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.position = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced: Displace center orientation (co)variance circle arrows along their rotation axis.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale: Scaling of pose gaussians (orientation part).
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale = 100
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame: If true, show global poses. If false, show poses in robot frame.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame = true
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames: Enable showing object frames.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale: Scaling of object frames.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs: Enable showing oriented bounding boxes.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.ghostAlpha: Alpha of linear prediction ghosts.
+# Attributes:
+# - Default: 0.699999988
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.ghostAlpha = 0.699999988
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow: Show arrows from current object poses to the linearly predicted ones.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame: Show frames at linearly predicted object poses.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost: Show ghosts at linearly predicted object poses.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset: The offset (in seconds) to the current time to make predictions for.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeWindow: The time window (in seconds) into the past to perform the regression on.
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeWindow = 2
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels: Prefer articulated object models if available.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
+
+
+# ArmarX.ObjectMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.ObjectMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.ObjectMemory.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.ObjectMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ObjectMemory.prediction.TimeWindow: Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.prediction.TimeWindow = 2
+
+
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
+# Attributes:
+# - Default: ObjectPoseTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic = ObjectPoseTopic
+
+
+# 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
+
+
diff --git a/scenarios/HumanAwareNavigation/config/RemoteGuiProviderApp.cfg b/scenarios/HumanAwareNavigation/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4b6abea40d72afd7d313ee47a9b191f3b26de30d
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/RemoteGuiProviderApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# RemoteGuiProviderApp 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.RemoteGuiProvider.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.RemoteGuiProvider.EnableProfiling = false
+
+
+# ArmarX.RemoteGuiProvider.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.RemoteGuiProvider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RemoteGuiProvider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.ObjectName = ""
+
+
+# ArmarX.RemoteGuiProvider.TopicName: Name of the topic on which updates to the remote state are reported.
+# Attributes:
+# - Default: RemoteGuiTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.TopicName = RemoteGuiTopic
+
+
+# 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
+
+
diff --git a/scenarios/HumanAwareNavigation/config/RobotStateComponent.cfg b/scenarios/HumanAwareNavigation/config/RobotStateComponent.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..07dc7425cd6d6b9c22936dbf2c15704d09bd78cc
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/RobotStateComponent.cfg
@@ -0,0 +1,308 @@
+# ==================================================================
+# RobotStateComponent 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.RobotStateComponent.AgentName: Name of the agent for which the sensor values are provided
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.RobotStateComponent.AgentName = Armar6
+
+
+# ArmarX.RobotStateComponent.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.RobotStateComponent.EnableProfiling = false
+
+
+# ArmarX.RobotStateComponent.GlobalRobotPoseLocalizationTopicName: Topic where the global robot pose can be reported.
+# Attributes:
+# - Default: GlobalRobotPoseLocalization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.GlobalRobotPoseLocalizationTopicName = GlobalRobotPoseLocalization
+
+
+# ArmarX.RobotStateComponent.HistoryLength: Number of entries in the robot state history
+# Attributes:
+# - Default: 10000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.HistoryLength = 10000
+
+
+# ArmarX.RobotStateComponent.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.RobotStateComponent.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotStateComponent.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.ObjectName = ""
+
+
+# ArmarX.RobotStateComponent.RobotFileName: Filename of VirtualRobot robot model (e.g. robot_model.xml)
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.RobotStateComponent.RobotFileName = Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml
+
+
+# ArmarX.RobotStateComponent.RobotModelScaling: Scaling of the robot model
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.RobotModelScaling = 1
+
+
+# ArmarX.RobotStateComponent.RobotNodeSetName: Set of nodes that is controlled by the KinematicUnit
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.RobotStateComponent.RobotNodeSetName = Robot
+
+
+# ArmarX.RobotStateComponent.RobotStateReportingTopic: Name of the topic on which updates of the robot state are reported.
+# Attributes:
+# - Default: RobotStateUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.RobotStateReportingTopic = RobotStateUpdates
+
+
+# ArmarX.RobotStateComponent.TopicPrefix: Prefix for the sensor value topic name.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.TopicPrefix = ""
+
+
+# ArmarX.RobotStateObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateObserver.CreateUpdateFrequenciesChannel = false
+
+
+# ArmarX.RobotStateObserver.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.RobotStateObserver.EnableProfiling = false
+
+
+# ArmarX.RobotStateObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.RobotStateObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateObserver.MaxHistorySize = 5000
+
+
+# ArmarX.RobotStateObserver.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.RobotStateObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotStateObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateObserver.ObjectName = ""
+
+
+# ArmarX.RobotStateObserver.TCPsToReport: comma seperated list of nodesets' endeffectors, which poses and velocities that should be reported. * for all, empty for none
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateObserver.TCPsToReport = ""
+
+
+# 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
+
+
diff --git a/scenarios/HumanAwareNavigation/config/SimpleVirtualRobot.cfg b/scenarios/HumanAwareNavigation/config/SimpleVirtualRobot.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..25864f55d0c37fa362228fce5577c51283acbef8
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/SimpleVirtualRobot.cfg
@@ -0,0 +1,255 @@
+# ==================================================================
+# SimpleVirtualRobot 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.SimpleVirtualRobot.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.SimpleVirtualRobot.EnableProfiling = false
+
+
+# ArmarX.SimpleVirtualRobot.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.SimpleVirtualRobot.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.SimpleVirtualRobot.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimpleVirtualRobot.ObjectName = ""
+
+
+# ArmarX.SimpleVirtualRobot.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.SimpleVirtualRobot.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.SimpleVirtualRobot.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimpleVirtualRobot.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.SimpleVirtualRobot.p.oneShot: If true, commit once after connecting, then stop.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.SimpleVirtualRobot.p.oneShot = true
+
+
+# ArmarX.SimpleVirtualRobot.p.robot.jointValues: Specify a certain joint configuration.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.SimpleVirtualRobot.p.robot.jointValues = { "tripod_joint_x":5000, "tripod_joint_y":0, "tripod_joint_rotation": -1.57, "mount_joint_height":250, "mount_joint_pitch":1.85}
+
+
+# ArmarX.SimpleVirtualRobot.p.robot.name: Optional override for the robot name. If not set, the default name from the robot model is used.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.SimpleVirtualRobot.p.robot.name = "AzureKinectCamera"
+
+
+# ArmarX.SimpleVirtualRobot.p.robot.package: Package of the Simox robot XML.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.SimpleVirtualRobot.p.robot.package = "PriorKnowledgeData"
+
+
+# ArmarX.SimpleVirtualRobot.p.robot.path: Local path of the Simox robot XML.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.SimpleVirtualRobot.p.robot.path = "robots/AzureKinectOnTripod/AzureKinectOnTripod.xml"
+
+
+# ArmarX.SimpleVirtualRobot.p.updateFrequency: Memory update frequency (write).
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimpleVirtualRobot.p.updateFrequency = 10
+
+
+# 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
+
+
diff --git a/scenarios/HumanAwareNavigation/config/VisionMemory.cfg b/scenarios/HumanAwareNavigation/config/VisionMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..1b95447f4edb52efc404a7bd9c2c00ee89ddf608
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/VisionMemory.cfg
@@ -0,0 +1,263 @@
+# ==================================================================
+# VisionMemory 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.VisionMemory.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.VisionMemory.EnableProfiling = false
+
+
+# ArmarX.VisionMemory.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.VisionMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.VisionMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.ObjectName = ""
+
+
+# ArmarX.VisionMemory.imageDepthMaxHistorySize:
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.imageDepthMaxHistorySize = 20
+
+
+# ArmarX.VisionMemory.imageRGBMaxHistorySize:
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.imageRGBMaxHistorySize = 20
+
+
+# ArmarX.VisionMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Vision
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mem.MemoryName = Vision
+
+
+# ArmarX.VisionMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.VisionMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.enabled = false
+
+
+# ArmarX.VisionMemory.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.VisionMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.VisionMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.VisionMemory.occupancyGridMaxHistorySize:
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.occupancyGridMaxHistorySize = 20
+
+
+# ArmarX.VisionMemory.pointCloudMaxHistorySize:
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.pointCloudMaxHistorySize = 20
+
+
diff --git a/scenarios/HumanAwareNavigation/config/XMLRemoteStateOfferer.NavigationGroup.cfg b/scenarios/HumanAwareNavigation/config/XMLRemoteStateOfferer.NavigationGroup.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..f366c706debe6c349f5bc4cdc7bb2b914ae528db
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/XMLRemoteStateOfferer.NavigationGroup.cfg
@@ -0,0 +1,227 @@
+# ==================================================================
+# XMLRemoteStateOfferer 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 = NavigationGroupApp
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# 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 = Debug
+
+
+# ArmarX.XMLStateComponent.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.XMLStateComponent.EnableProfiling = false
+
+
+# ArmarX.XMLStateComponent.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.XMLStateComponent.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.XMLStateComponent.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.XMLStateComponent.ObjectName = NavigationGroupXMLStateComponent
+
+
+# ArmarX.XMLStateComponent.StateReportingTopic: Topic on which state changes are published. Leave empty to disable reporting.
+# Attributes:
+# - Default: StateReportingTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.XMLStateComponent.StateReportingTopic = StateReportingTopic
+
+
+# ArmarX.XMLStateComponent.StatesToEnter: List of states that are directly entered and executed. Seperated by comma. These must not need any input parameters!
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.XMLStateComponent.StatesToEnter = NavigateToLocation
+
+
+# ArmarX.XMLStateComponent.XMLRemoteStateOffererName: Name of the RemoteStateOfferer to start. The default name is [StatechartGroupName]RemoteStateOfferer.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.XMLStateComponent.XMLRemoteStateOffererName = ""
+
+
+# ArmarX.XMLStateComponent.XMLStatechartGroupDefinitionFile: Path to statechart group definition file (*.scgxml) - relative to projects' source dir
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.XMLStateComponent.XMLStatechartGroupDefinitionFile = armarx/navigation/statecharts/NavigationGroup/NavigationGroup.scgxml
+
+
+# ArmarX.XMLStateComponent.XMLStatechartProfile: Profile to use for state execution. Should be the same for all Statechart Groups in one scenario. This profile name is also added as a prefix to the Ice identifier of the RemoteStateOfferer of this statechart group.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.XMLStateComponent.XMLStatechartProfile = Armar3a
+
+
diff --git a/scenarios/HumanAwareNavigation/config/control_memory.cfg b/scenarios/HumanAwareNavigation/config/control_memory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..53efd6fedf80402149271390c3c93fca3e434c63
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/control_memory.cfg
@@ -0,0 +1,272 @@
+# ==================================================================
+# control_memory 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.ControlMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.ControlMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ControlMemory.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.ControlMemory.EnableProfiling = false
+
+
+# ArmarX.ControlMemory.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.ControlMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ControlMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.ObjectName = ""
+
+
+# ArmarX.ControlMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.ControlMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Control
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.MemoryName = Control
+
+
+# ArmarX.ControlMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.ControlMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.enabled = false
+
+
+# ArmarX.ControlMemory.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.ControlMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ControlMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ControlMemory.p.locationGraph.visuFrequency: Visualization frequeny of locations and graph edges [Hz].
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.p.locationGraph.visuFrequency = 2
+
+
+# ArmarX.ControlMemory.p.snapshotToLoad: Memory snapshot to load at start up
+# (e.g. 'PriorKnowledgeData/navigation-graphs/snapshot').
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.p.snapshotToLoad = ""
+
+
+# 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
+
+
diff --git a/scenarios/HumanAwareNavigation/config/distance_to_obstacle_costmap_provider.cfg b/scenarios/HumanAwareNavigation/config/distance_to_obstacle_costmap_provider.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..6059c7a85d41e91d2c9daffbe3ea5a3913898b9b
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/distance_to_obstacle_costmap_provider.cfg
@@ -0,0 +1,262 @@
+# ==================================================================
+# distance_to_obstacle_costmap_provider 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.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.EnableProfiling = false
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectName = ""
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory = Navigation
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider = ""
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.p.robotName: Robot name.
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.p.robotName = Armar6
+
+
diff --git a/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg b/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..81d7cebf824840bbff1c4d28da4cebcfabc95977
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
@@ -0,0 +1,309 @@
+# ==================================================================
+# dynamic_distance_to_obstacle_costmap_provider 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.dynamic_distance_to_obstacle_costmap_provider.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.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.dynamic_distance_to_obstacle_costmap_provider.EnableProfiling = false
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.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.dynamic_distance_to_obstacle_costmap_provider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.ObjectName = ""
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory = Navigation
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider = ""
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.CoreSegment: Name of the mapping memory core segment to use.
+# Attributes:
+# - Default: LaserScannerFeatures
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.CoreSegment = LaserScannerFeatures
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.MemoryName:
+# Attributes:
+# - Default: Vision
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.MemoryName = Vision
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.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.dynamic_distance_to_obstacle_costmap_provider.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.name:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.name = ""
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.providerName:
+# Attributes:
+# - Default: LaserScannerFeatureExtraction
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.providerName = LaserScannerFeatureExtraction
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.robot.name:
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.robot.name = Armar6
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.name:
+# Attributes:
+# - Default: distance_to_obstacles
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.name = distance_to_obstacles
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.providerName:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.providerName = distance_to_obstacle_costmap_provider
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.updatePeriodMs:
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.updatePeriodMs = 100
+
+
diff --git a/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg b/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..308897838468d989f142a5097329699d68f58a21
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
@@ -0,0 +1,406 @@
+# ==================================================================
+# dynamic_scene_provider 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.dynamic_scene_provider.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.dynamic_scene_provider.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.dynamic_scene_provider.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.dynamic_scene_provider.EnableProfiling = false
+
+
+# ArmarX.dynamic_scene_provider.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.dynamic_scene_provider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.dynamic_scene_provider.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.dynamic_scene_provider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ObjectName = ""
+
+
+# ArmarX.dynamic_scene_provider.mem.human.pose.CoreSegment:
+# Attributes:
+# - Default: Pose
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.human.pose.CoreSegment = Pose
+
+
+# ArmarX.dynamic_scene_provider.mem.human.pose.Memory:
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.human.pose.Memory = Human
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.costmap.Memory = Navigation
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.human.CoreSegment:
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human.CoreSegment = Human
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.human.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human.Memory = Navigation
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.human.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human.Provider = ""
+
+
+# ArmarX.dynamic_scene_provider.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.dynamic_scene_provider.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.dynamic_scene_provider.mem.vision.laser_scanner_features.CoreSegment: Name of the mapping memory core segment to use.
+# Attributes:
+# - Default: LaserScannerFeatures
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.vision.laser_scanner_features.CoreSegment = LaserScannerFeatures
+
+
+# ArmarX.dynamic_scene_provider.mem.vision.laser_scanner_features.MemoryName:
+# Attributes:
+# - Default: Vision
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.vision.laser_scanner_features.MemoryName = Vision
+
+
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.CoreSegment:
+# Attributes:
+# - Default: OccupancyGrid
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.CoreSegment = OccupancyGrid
+
+
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.Memory:
+# Attributes:
+# - Default: Vision
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.Memory = Vision
+
+
+# ArmarX.dynamic_scene_provider.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.dynamic_scene_provider.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.dynamic_scene_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.dynamic_scene_provider.p.humanPoseProvider:
+# Attributes:
+# - Default: AzureKinectPointCloudProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.humanPoseProvider = AzureKinectPointCloudProvider
+
+
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.name:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.name = ""
+
+
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.providerName:
+# Attributes:
+# - Default: LaserScannerFeatureExtraction
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.providerName = LaserScannerFeatureExtraction
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.freespaceThreshold:
+# Attributes:
+# - Default: 0.449999988
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.freespaceThreshold = 0.449999988
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.name:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.name = ""
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.occupiedThreshold:
+# Attributes:
+# - Default: 0.550000012
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.occupiedThreshold = 0.550000012
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.providerName:
+# Attributes:
+# - Default: CartographerMappingAndLocalization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.providerName = CartographerMappingAndLocalization
+
+
+# ArmarX.dynamic_scene_provider.p.robot.name:
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.robot.name = Armar6
+
+
+# ArmarX.dynamic_scene_provider.p.taskPeriodMs: Update rate of the running task.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.taskPeriodMs = 100
+
+
diff --git a/scenarios/HumanAwareNavigation/config/example_client.cfg b/scenarios/HumanAwareNavigation/config/example_client.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..680e39ca947c7539fbcb1bd97c63057259f0b54a
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/example_client.cfg
@@ -0,0 +1,254 @@
+# ==================================================================
+# example_client 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.ExampleClient.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.ExampleClient.EnableProfiling = false
+
+
+# ArmarX.ExampleClient.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.ExampleClient.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ExampleClient.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleClient.ObjectName = ""
+
+
+# ArmarX.ExampleClient.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleClient.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.ExampleClient.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleClient.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.ExampleClient.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.ExampleClient.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ExampleClient.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleClient.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ExampleClient.nav.NavigatorName: Name of the Navigator
+# Attributes:
+# - Default: navigator
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ExampleClient.nav.NavigatorName = navigator
+
+
+# ArmarX.ExampleClient.relativeMovement: The distance between two target poses [mm]
+# Attributes:
+# - Default: 200
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleClient.relativeMovement = 200
+
+
+# ArmarX.ExampleClient.robotName:
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleClient.robotName = Armar6
+
+
+# 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.example_client.mode: No Description
+# Attributes:
+# - Default: wander_around
+# - Case sensitivity: no
+# - Required: no
+ArmarX.example_client.mode = wander_around
+
+
diff --git a/scenarios/HumanAwareNavigation/config/global.cfg b/scenarios/HumanAwareNavigation/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..b127ca30a5c577086e8cf0aaf6dec8e5858fea7b
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario HumanAwareNavigation
+# ==================================================================
+
diff --git a/scenarios/HumanAwareNavigation/config/human_simulator.cfg b/scenarios/HumanAwareNavigation/config/human_simulator.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..b628b0a4348fe7d53c90652775ccf1f61121c582
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/human_simulator.cfg
@@ -0,0 +1,14 @@
+# ==================================================================
+# human_simulator properties
+# ==================================================================
+
+# ArmarX.human_simulator.p.nHumans:
+# Attributes:
+ArmarX.human_simulator.p.nHumans = 3
+
+
+# ArmarX.human_simulator.p.taskPeriodMs:
+# Attributes:
+ArmarX.human_simulator.p.taskPeriodMs = 100
+
+
diff --git a/scenarios/HumanAwareNavigation/config/navigation_memory.cfg b/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..45901d4483977812ccf754fd73b2d6ce7fd8a3c7
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
@@ -0,0 +1,290 @@
+# ==================================================================
+# navigation_memory 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 = Verbose
+
+
+# ArmarX.navigation_memory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.navigation_memory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.navigation_memory.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.navigation_memory.EnableProfiling = false
+
+
+# ArmarX.navigation_memory.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.navigation_memory.MinimumLoggingLevel = Verbose
+
+
+# ArmarX.navigation_memory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.ObjectName = ""
+
+
+# ArmarX.navigation_memory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.navigation_memory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.mem.MemoryName = Navigation
+
+
+# ArmarX.navigation_memory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.mem.ltm.configuration = {}
+
+
+# ArmarX.navigation_memory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.mem.ltm.enabled = false
+
+
+# ArmarX.navigation_memory.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.navigation_memory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.navigation_memory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.navigation_memory.p.locationGraph.visuFrequency: Visualization frequeny of locations and graph edges [Hz].
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.p.locationGraph.visuFrequency = 10
+
+
+# ArmarX.navigation_memory.p.locationGraph.visuGraphEdges: Enable visualization of navigation graph edges.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.locationGraph.visuGraphEdges = true
+
+
+# ArmarX.navigation_memory.p.locationGraph.visuLocation: Enable visualization of locations.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.locationGraph.visuLocation = true
+
+
+# ArmarX.navigation_memory.p.snapshotToLoad: Memory snapshot to load at start up
+# (e.g. 'PriorKnowledgeData/navigation-graphs/snapshot').
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.p.snapshotToLoad = ""
+
+
diff --git a/scenarios/HumanAwareNavigation/config/navigator.cfg b/scenarios/HumanAwareNavigation/config/navigator.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..48abd6368a620a58fc54040ee4715b23aceeec9c
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/navigator.cfg
@@ -0,0 +1,437 @@
+# ==================================================================
+# navigator 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.navigator.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.navigator.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.navigator.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.navigator.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.navigator.EnableProfiling = false
+
+
+# ArmarX.navigator.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.navigator.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.navigator.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.navigator.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.ObjectName = ""
+
+
+# ArmarX.navigator.RobotUnitName: Name of the RobotUnit
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.navigator.RobotUnitName = Armar6Unit
+
+
+# ArmarX.navigator.cmp.RemoteGui: Ice object name of the `RemoteGui` component.
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.cmp.RemoteGui = RemoteGuiProvider
+
+
+# ArmarX.navigator.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.navigator.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.costmap.Memory = Navigation
+
+
+# ArmarX.navigator.mem.nav.events.CoreSegment:
+# Attributes:
+# - Default: Events
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.events.CoreSegment = Events
+
+
+# ArmarX.navigator.mem.nav.events.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.events.Memory = Navigation
+
+
+# ArmarX.navigator.mem.nav.events.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.events.Provider = ""
+
+
+# ArmarX.navigator.mem.nav.graph.CoreSegment:
+# Attributes:
+# - Default: Location
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.graph.CoreSegment = Location
+
+
+# ArmarX.navigator.mem.nav.graph.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.graph.Memory = Navigation
+
+
+# ArmarX.navigator.mem.nav.human.CoreSegment:
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.human.CoreSegment = Human
+
+
+# ArmarX.navigator.mem.nav.human.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.human.Memory = Navigation
+
+
+# ArmarX.navigator.mem.nav.param.CoreSegment:
+# Attributes:
+# - Default: Parameterization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.param.CoreSegment = Parameterization
+
+
+# ArmarX.navigator.mem.nav.param.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.param.Memory = Navigation
+
+
+# ArmarX.navigator.mem.nav.param.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.param.Provider = ""
+
+
+# ArmarX.navigator.mem.nav.stack_result.CoreSegment:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.stack_result.CoreSegment = ""
+
+
+# ArmarX.navigator.mem.nav.stack_result.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.stack_result.Memory = Navigation
+
+
+# ArmarX.navigator.mem.nav.stack_result.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.stack_result.Provider = ""
+
+
+# ArmarX.navigator.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.navigator.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.navigator.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.navigator.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.navigator.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.navigator.p.disableExecutor: If the executor is disabled, the navigator will only plan the trajectory but won't execute it.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigator.p.disableExecutor = false
+
+
+# ArmarX.navigator.p.occupancy_grid.occopied_threshold: Threshold for each cell to be considered occupied. Increase this value to reduce noise.
+# Attributes:
+# - Default: 0.550000012
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.navigator.p.occupancy_grid.occopied_threshold = 0.8
+
+
+# ArmarX.navigator.p.scene.humanProviderName:
+# Attributes:
+# - Default: dynamic_scene_provider
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.navigator.p.scene.humanProviderName = human_simulator
+
+
+# ArmarX.navigator.p.scene.robotName:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.navigator.p.scene.robotName = Armar6
+
+
+# ArmarX.navigator.p.scene.staticCostmapName:
+# Attributes:
+# - Default: distance_to_obstacles
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.p.scene.staticCostmapName = distance_to_obstacles
+
+
+# ArmarX.navigator.p.scene.staticCostmapProviderName:
+# Attributes:
+# - Default: distance_to_obstacle_costmap_provider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.p.scene.staticCostmapProviderName = distance_to_obstacle_costmap_provider
+
+
diff --git a/scenarios/NavigationSimulation/NavigationSimulation.scx b/scenarios/NavigationSimulation/NavigationSimulation.scx
index 66b83347a8ca47c34f40633555c1207362b0adee..535b4bdde90046ade0320dc55d287545ecb6bcf0 100644
--- a/scenarios/NavigationSimulation/NavigationSimulation.scx
+++ b/scenarios/NavigationSimulation/NavigationSimulation.scx
@@ -3,12 +3,7 @@
<application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ConditionHandler" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="SystemObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="CommonStorage" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="PriorKnowledge" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="LongtermMemory" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="WorkingMemory" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ViewSelectionApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="ObjectLocalizationSaliencyApp" instance="" package="RobotComponents" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="SimulatorApp" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="SimulatorViewerApp" instance="" package="ArmarXSimulation" nodeName="" enabled="false" iceAutoRestart="false"/>
@@ -19,25 +14,19 @@
<application name="DepthImageProviderDynamicSimulationApp" instance="" package="ArmarXSimulation" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="ObjectLocalizationDynamicSimulationApp" instance="TexturedRecognition" package="ArmarXSimulation" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="ObjectLocalizationDynamicSimulationApp" instance="SegmentableRecognition" package="ArmarXSimulation" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="ObjectMemoryObserver" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="HandUnitDynamicSimulationApp" instance="LeftHand" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="HandUnitDynamicSimulationApp" instance="RightHand" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ForceTorqueObserver" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="SelfLocalizationDynamicSimulationApp" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotHandLocalizationDynamicSimulationApp" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="HeadIKUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="TCPControlUnit" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="ProfilerObserverApp" instance="" package="ArmarXCore" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="GraphNodePoseResolverApp" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ImageSourceSelectionApp" instance="" package="VisionX" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="RobotUnitSimulationApp" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="LaserScannerUnitObserverApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ImageProviderDynamicSimulationApp" instance="Roboception" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArmarXFileLoggerApp" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="WorkingMemoryToArVizApp" instance="" package="MemoryX" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="ObstacleAvoidingPlatformUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="DebugDrawerToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArticulatedObjectLocalizerDynamicSimulation" instance="" package="ArmarXSimulation" nodeName="" enabled="false" iceAutoRestart="false"/>
@@ -48,5 +37,9 @@
<application name="LaserScannerPointCloudProviderApp" instance="" package="VisionX" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="LaserScannerSimulation" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="VisionMemory" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="HumanMemoryApp" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="object_memory_to_simulation" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="SimulationObjectPoseProviderApp" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="control_memory" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/NavigationSimulation/config/HumanMemoryApp.cfg b/scenarios/NavigationSimulation/config/HumanMemoryApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..28099ebc83514ac57b5af9e5acba37e72a6f3846
--- /dev/null
+++ b/scenarios/NavigationSimulation/config/HumanMemoryApp.cfg
@@ -0,0 +1,368 @@
+# ==================================================================
+# HumanMemoryApp 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.HumanMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.HumanMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.HumanMemory.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.HumanMemory.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.HumanMemory.EnableProfiling = false
+
+
+# ArmarX.HumanMemory.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.HumanMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.HumanMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ObjectName = ""
+
+
+# ArmarX.HumanMemory.face.seg.CoreMaxHistorySize: Maximal size of the FaceRecognition entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.face.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.HumanMemory.face.seg.CoreSegmentName: Name of the FaceRecognition core segment.
+# Attributes:
+# - Default: FaceRecognition
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.face.seg.CoreSegmentName = FaceRecognition
+
+
+# ArmarX.HumanMemory.ident.seg.CoreMaxHistorySize: Maximal size of the Identification entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ident.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.HumanMemory.ident.seg.CoreSegmentName: Name of the Identification core segment.
+# Attributes:
+# - Default: Identification
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ident.seg.CoreSegmentName = Identification
+
+
+# ArmarX.HumanMemory.instanceseg.CoreMaxHistorySize: Maximal size of the PersonInstance entity histories (-1 for infinite).
+# Attributes:
+# - Default: 32
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.instanceseg.CoreMaxHistorySize = 32
+
+
+# ArmarX.HumanMemory.instanceseg.CoreSegmentName: Name of the PersonInstance core segment.
+# Attributes:
+# - Default: PersonInstance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.instanceseg.CoreSegmentName = PersonInstance
+
+
+# ArmarX.HumanMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.MemoryName = Human
+
+
+# ArmarX.HumanMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.HumanMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HumanMemory.mem.ltm.enabled = false
+
+
+# ArmarX.HumanMemory.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.HumanMemory.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.HumanMemory.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.HumanMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.HumanMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.HumanMemory.pose.seg.CoreMaxHistorySize: Maximal size of the Pose entity histories (-1 for infinite).
+# Attributes:
+# - Default: 256
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.pose.seg.CoreMaxHistorySize = 256
+
+
+# ArmarX.HumanMemory.pose.seg.CoreSegmentName: Name of the Pose core segment.
+# Attributes:
+# - Default: Pose
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.pose.seg.CoreSegmentName = Pose
+
+
+# ArmarX.HumanMemory.profile.pk.load: Load profiles from prior knowledge on startup.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HumanMemory.profile.pk.load = true
+
+
+# ArmarX.HumanMemory.profile.pk.packageName: ArmarX package to load human profiles from.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.pk.packageName = PriorKnowledgeData
+
+
+# ArmarX.HumanMemory.profile.seg.CoreMaxHistorySize: Maximal size of the Profile entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.HumanMemory.profile.seg.CoreSegmentName: Name of the Profile core segment.
+# Attributes:
+# - Default: Profile
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.seg.CoreSegmentName = Profile
+
+
+# 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
+
+
diff --git a/scenarios/NavigationSimulation/config/ObjectLocalizationDynamicSimulationApp.instance2.cfg b/scenarios/NavigationSimulation/config/ObjectLocalizationDynamicSimulationApp.instance2.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..5bc037587a3b0ec12ad2ca34fbe5bc496e833e37
--- /dev/null
+++ b/scenarios/NavigationSimulation/config/ObjectLocalizationDynamicSimulationApp.instance2.cfg
@@ -0,0 +1,229 @@
+# ==================================================================
+# ObjectLocalizationDynamicSimulationApp 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.ObjectLocalizerDynamicSimulation.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.ObjectLocalizerDynamicSimulation.EnableProfiling = false
+
+
+# ArmarX.ObjectLocalizerDynamicSimulation.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.ObjectLocalizerDynamicSimulation.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectLocalizerDynamicSimulation.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectLocalizerDynamicSimulation.ObjectName = ""
+
+
+# ArmarX.ObjectLocalizerDynamicSimulation.RecognitionCertainty: Certainty of recognition used in simulation (0...1).
+# Attributes:
+# - Default: 0.899999976
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectLocalizerDynamicSimulation.RecognitionCertainty = 0.899999976
+
+
+# ArmarX.ObjectLocalizerDynamicSimulation.ReferenceFrameName: Name of reference frame to use for pose
+# Attributes:
+# - Default: EyeLeftCamera
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectLocalizerDynamicSimulation.ReferenceFrameName = EyeLeftCamera
+
+
+# ArmarX.ObjectLocalizerDynamicSimulation.RobotName: Name of robot used for calculating reference frame
+# Attributes:
+# - Default: Armar3
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectLocalizerDynamicSimulation.RobotName = Armar3
+
+
+# ArmarX.ObjectLocalizerDynamicSimulation.SimulatorProxyName: name of dynamics simulator proxy
+# Attributes:
+# - Default: Simulator
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectLocalizerDynamicSimulation.SimulatorProxyName = Simulator
+
+
+# ArmarX.ObjectLocalizerDynamicSimulation.VisibilityCheck: Whether to simulate camera and checking visibility within camera
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectLocalizerDynamicSimulation.VisibilityCheck = false
+
+
+# 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
+
+
diff --git a/scenarios/NavigationSimulation/config/ObjectMemory.cfg b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
index 6811e284df9a2277231f7f89c55232407c91a92d..5984b8da6ad696e2d67a7b1e0b360897fd9d5f5e 100644
--- a/scenarios/NavigationSimulation/config/ObjectMemory.cfg
+++ b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
@@ -391,9 +391,12 @@ ArmarX.ObjectMemory.MinimumLoggingLevel = Debug
# ArmarX.ObjectMemory.mem.inst.scene.11_Directory = scenes
-# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad: Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').
-# You can also specify paths relative to 'Package/scenes/'.
-# You can also specify a ; separated list of scenes.
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad: Scene(s) to load on startup.
+# Specify multiple scenes in a ; separated list.
+# Each entry must be one of the following:
+# (1) A scene file in 'Package/scenes/' (with or without '.json' extension), e.g. 'MyScene', 'MyScene.json'
+# (2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' extension), e.g. 'path/to/MyScene', 'path/to/MyScene.json'
+# (3) An ArmarX data path to a scene file, e.g. 'Package/scenes/path/to/MyScene.json'
# Attributes:
# - Default: ""
# - Case sensitivity: yes
diff --git a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
index d7b57fa58aae07e4bb718deb2c5b31713120e62e..d3524d3d8bdb280fee2ff7887346ef075d005124 100644
--- a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
+++ b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
@@ -223,7 +223,7 @@ ArmarX.SelfLocalizationDynamicSimulation.longterm_memory.retrieve_initial_pose =
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SelfLocalizationDynamicSimulation.longterm_memory.update = true
+ArmarX.SelfLocalizationDynamicSimulation.longterm_memory.update = false
# ArmarX.SelfLocalizationDynamicSimulation.longterm_memory.update_frequency:
@@ -322,7 +322,7 @@ ArmarX.SelfLocalizationDynamicSimulation.mem.robot_state.Memory = RobotState
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SelfLocalizationDynamicSimulation.working_memory.update = true
+ArmarX.SelfLocalizationDynamicSimulation.working_memory.update = false
# ArmarX.SelfLocalizationDynamicSimulation.working_memory.update_frequency:
diff --git a/scenarios/NavigationSimulation/config/SimulationObjectPoseProviderApp.cfg b/scenarios/NavigationSimulation/config/SimulationObjectPoseProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4121ec6ab4668078edfba4de612bfb28a4a25a7c
--- /dev/null
+++ b/scenarios/NavigationSimulation/config/SimulationObjectPoseProviderApp.cfg
@@ -0,0 +1,229 @@
+# ==================================================================
+# SimulationObjectPoseProviderApp 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.SimulationObjectPoseProvider.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.SimulationObjectPoseProvider.EnableProfiling = false
+
+
+# ArmarX.SimulationObjectPoseProvider.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.SimulationObjectPoseProvider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.SimulationObjectPoseProvider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimulationObjectPoseProvider.ObjectName = ""
+
+
+# ArmarX.SimulationObjectPoseProvider.ObjectPoseTopicName: Name of the object pose topic.
+# Attributes:
+# - Default: ObjectPoseTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimulationObjectPoseProvider.ObjectPoseTopicName = ObjectPoseTopic
+
+
+# ArmarX.SimulationObjectPoseProvider.UpdateFrequency: Frequency at which to update the objects in the ObjectMemory.
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimulationObjectPoseProvider.UpdateFrequency = 10
+
+
+# ArmarX.SimulationObjectPoseProvider.cmp.Simulator: Ice object name of the `Simulator` component.
+# Attributes:
+# - Default: Simulator
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimulationObjectPoseProvider.cmp.Simulator = Simulator
+
+
+# ArmarX.SimulationObjectPoseProvider.p.initiallyRequestedEntities: All entities (comma separated) that should be requested from the beginning. If you want an entity to be localized for n seconds append ':n'.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimulationObjectPoseProvider.p.initiallyRequestedEntities = ""
+
+
+# ArmarX.SimulationObjectPoseProvider.p.requestAllEntities: True if all entities should be requested all the time.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+ArmarX.SimulationObjectPoseProvider.p.requestAllEntities = 1
+
+
+# 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
+
+
diff --git a/scenarios/NavigationSimulation/config/SimulatorApp.cfg b/scenarios/NavigationSimulation/config/SimulatorApp.cfg
index 8401b4d00a8c7b13b6ff734105ba9a0ce2539a47..d7630d26ee3918b02e04afbfb6400210b2f98cba 100644
--- a/scenarios/NavigationSimulation/config/SimulatorApp.cfg
+++ b/scenarios/NavigationSimulation/config/SimulatorApp.cfg
@@ -289,7 +289,7 @@ ArmarX.Simulator.InitialRobotConfig = ArmL1_Cla1:0.036781001836061478,ArmL2_Sho1
# - Default: 0
# - Case sensitivity: yes
# - Required: no
-ArmarX.Simulator.InitialRobotPose.x = 4000
+ArmarX.Simulator.InitialRobotPose.x = 2000
# ArmarX.Simulator.InitialRobotPose.x_0: x component of initial robot position (mm)
@@ -900,7 +900,7 @@ ArmarX.Simulator.SimulationType = kinematics
# - Default: 25
# - Case sensitivity: yes
# - Required: no
-ArmarX.Simulator.StepTimeMS = 10
+ArmarX.Simulator.StepTimeMS = 1
# ArmarX.Simulator.WorkingMemoryName: Name of WorkingMemory
diff --git a/scenarios/NavigationSimulation/config/control_memory.cfg b/scenarios/NavigationSimulation/config/control_memory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..53efd6fedf80402149271390c3c93fca3e434c63
--- /dev/null
+++ b/scenarios/NavigationSimulation/config/control_memory.cfg
@@ -0,0 +1,272 @@
+# ==================================================================
+# control_memory 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.ControlMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.ControlMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ControlMemory.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.ControlMemory.EnableProfiling = false
+
+
+# ArmarX.ControlMemory.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.ControlMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ControlMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.ObjectName = ""
+
+
+# ArmarX.ControlMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.ControlMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Control
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.MemoryName = Control
+
+
+# ArmarX.ControlMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.ControlMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.enabled = false
+
+
+# ArmarX.ControlMemory.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.ControlMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ControlMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ControlMemory.p.locationGraph.visuFrequency: Visualization frequeny of locations and graph edges [Hz].
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.p.locationGraph.visuFrequency = 2
+
+
+# ArmarX.ControlMemory.p.snapshotToLoad: Memory snapshot to load at start up
+# (e.g. 'PriorKnowledgeData/navigation-graphs/snapshot').
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.p.snapshotToLoad = ""
+
+
+# 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
+
+
diff --git a/scenarios/NavigationSimulation/config/object_memory_to_simulation.cfg b/scenarios/NavigationSimulation/config/object_memory_to_simulation.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..f3deca0864b51f08b251d95ea5b30cc89c27513d
--- /dev/null
+++ b/scenarios/NavigationSimulation/config/object_memory_to_simulation.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# object_memory_to_simulation 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.ObjectMemoryToSimulation.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.ObjectMemoryToSimulation.EnableProfiling = false
+
+
+# ArmarX.ObjectMemoryToSimulation.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.ObjectMemoryToSimulation.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectMemoryToSimulation.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemoryToSimulation.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.ObjectMemoryToSimulation.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemoryToSimulation.ObjectName = ""
+
+
+# ArmarX.ObjectMemoryToSimulation.cmp.Simulator: Ice object name of the `Simulator` component.
+# Attributes:
+# - Default: Simulator
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemoryToSimulation.cmp.Simulator = Simulator
+
+
+# ArmarX.ObjectMemoryToSimulation.p.objectPoseProviderName:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemoryToSimulation.p.objectPoseProviderName = ""
+
+
+# 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
+
+
diff --git a/scenarios/PlatformNavigation/PlatformNavigation.scx b/scenarios/PlatformNavigation/PlatformNavigation.scx
index 1c803ab345fb9587cc15a9749305a7416262235d..5bce30456bff46714c9d10d30f69ec792f322e42 100644
--- a/scenarios/PlatformNavigation/PlatformNavigation.scx
+++ b/scenarios/PlatformNavigation/PlatformNavigation.scx
@@ -9,6 +9,8 @@
<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"/>
+ <application name="dynamic_distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="navigation_skill_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="SkillsMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/PlatformNavigation/config/distance_to_obstacle_costmap_provider.cfg b/scenarios/PlatformNavigation/config/distance_to_obstacle_costmap_provider.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..6059c7a85d41e91d2c9daffbe3ea5a3913898b9b
--- /dev/null
+++ b/scenarios/PlatformNavigation/config/distance_to_obstacle_costmap_provider.cfg
@@ -0,0 +1,262 @@
+# ==================================================================
+# distance_to_obstacle_costmap_provider 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.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.EnableProfiling = false
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectName = ""
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory = Navigation
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider = ""
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.p.robotName: Robot name.
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.p.robotName = Armar6
+
+
diff --git a/scenarios/PlatformNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg b/scenarios/PlatformNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..81d7cebf824840bbff1c4d28da4cebcfabc95977
--- /dev/null
+++ b/scenarios/PlatformNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
@@ -0,0 +1,309 @@
+# ==================================================================
+# dynamic_distance_to_obstacle_costmap_provider 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.dynamic_distance_to_obstacle_costmap_provider.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.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.dynamic_distance_to_obstacle_costmap_provider.EnableProfiling = false
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.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.dynamic_distance_to_obstacle_costmap_provider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.ObjectName = ""
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory = Navigation
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider = ""
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.CoreSegment: Name of the mapping memory core segment to use.
+# Attributes:
+# - Default: LaserScannerFeatures
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.CoreSegment = LaserScannerFeatures
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.MemoryName:
+# Attributes:
+# - Default: Vision
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.MemoryName = Vision
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.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.dynamic_distance_to_obstacle_costmap_provider.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.name:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.name = ""
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.providerName:
+# Attributes:
+# - Default: LaserScannerFeatureExtraction
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.providerName = LaserScannerFeatureExtraction
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.robot.name:
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.robot.name = Armar6
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.name:
+# Attributes:
+# - Default: distance_to_obstacles
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.name = distance_to_obstacles
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.providerName:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.providerName = distance_to_obstacle_costmap_provider
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.updatePeriodMs:
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.updatePeriodMs = 100
+
+
diff --git a/scenarios/PlatformNavigation/config/example_client.cfg b/scenarios/PlatformNavigation/config/example_client.cfg
index eef853017b53a47640988642eec0e92c8660c615..79693c7b9fd2f793e80e640804ab8ac10ffd1b64 100644
--- a/scenarios/PlatformNavigation/config/example_client.cfg
+++ b/scenarios/PlatformNavigation/config/example_client.cfg
@@ -76,171 +76,204 @@
# ArmarX.EnableProfiling = false
-# ArmarX.ExampleClient.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ExampleClient.nav.NavigatorName: No Description
# Attributes:
-# - Default: false
-# - Case sensitivity: yes
+# - Default: navigator
+# - Case sensitivity: no
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleClient.EnableProfiling = false
+ArmarX.ExampleClient.nav.NavigatorName = navigator
-# ArmarX.ExampleClient.MinimumLoggingLevel: Local logging level only for this component
+# 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: Undefined
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.ExampleClient.MinimumLoggingLevel = Undefined
+# ArmarX.LoadLibraries = ""
-# ArmarX.ExampleClient.ObjectName: Name of IceGrid well-known object
+# 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.ExampleClient.ObjectName = ""
+# ArmarX.LoggingGroup = ""
-# ArmarX.ExampleClient.mem.robot_state.Memory:
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
-# - Default: RobotState
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.mem.robot_state.Memory = RobotState
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
-# ArmarX.ExampleClient.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# 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: Localization
+# - Default: 3000
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.mem.robot_state.localizationSegment = Localization
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.ExampleClient.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
-# - Default: true
+# - 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.ExampleClient.mns.MemoryNameSystemEnabled = true
+# ArmarX.StartDebuggerOnCrash = false
-# ArmarX.ExampleClient.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
# Attributes:
-# - Default: MemoryNameSystem
+# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.ThreadPoolSize = 1
-# ArmarX.ExampleClient.nav.NavigatorName: Name of the Navigator
+# 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: navigator
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.ExampleClient.nav.NavigatorName = navigator
+# ArmarX.TopicSuffix = ""
-# ArmarX.ExampleClient.relativeMovement: The distance between two target poses [mm]
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
-# - Default: 200
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.relativeMovement = 200
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
-# ArmarX.ExampleClient.robotName:
+# ArmarX.Verbosity: Global logging level for whole application
# Attributes:
-# - Default: Armar6
+# - Default: Info
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.robotName = Armar6
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
-# 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;...
+# ArmarX.example_client.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: ""
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.LoadLibraries = ""
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.example_client.EnableProfiling = false
-# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# ArmarX.example_client.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.example_client.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.example_client.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.LoggingGroup = ""
+# ArmarX.example_client.ObjectName = ""
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# ArmarX.example_client.mem.nav.costmap.CoreSegment:
# Attributes:
-# - Default: true
+# - Default: Costmap
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
+# ArmarX.example_client.mem.nav.costmap.CoreSegment = Costmap
-# 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)
+# ArmarX.example_client.mem.nav.costmap.Memory:
# Attributes:
-# - Default: 3000
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
+# ArmarX.example_client.mem.nav.costmap.Memory = Navigation
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# ArmarX.example_client.mem.robot_state.Memory:
# Attributes:
-# - Default: 0
+# - Default: RobotState
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SecondsStartupDelay = 0
+# ArmarX.example_client.mem.robot_state.Memory = RobotState
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# ArmarX.example_client.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
# Attributes:
-# - Default: false
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.example_client.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.StartDebuggerOnCrash = false
+# ArmarX.example_client.mns.MemoryNameSystemEnabled = true
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# ArmarX.example_client.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: 1
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ThreadPoolSize = 1
+# ArmarX.example_client.mns.MemoryNameSystemName = MemoryNameSystem
-# 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.
+# ArmarX.example_client.mode: Which example to run
# Attributes:
-# - Default: ""
+# - Default: standard
# - Case sensitivity: yes
# - Required: no
-# ArmarX.TopicSuffix = ""
+# - Possible values: {complex, point_to_point, standard, update_navigator, wander_around}
+ArmarX.example_client.mode = standard
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# ArmarX.example_client.nav.NavigatorName: Name of the Navigator
# Attributes:
-# - Default: false
+# - Default: navigator
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
+# ArmarX.example_client.nav.NavigatorName = navigator
-# ArmarX.Verbosity: Global logging level for whole application
+# ArmarX.example_client.relativeMovement: The distance between two target poses [mm]
# Attributes:
-# - Default: Info
+# - Default: 200
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
+# ArmarX.example_client.relativeMovement = 200
+
+
+# ArmarX.example_client.robotName:
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.robotName = Armar6
diff --git a/scenarios/PlatformNavigation/config/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index 8945e9ca86ae9055750010e93a7ae1f88e4db55b..37ca71940acfbf598f307c760fc41765811c3eb0 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -92,199 +92,207 @@
# ArmarX.LoggingGroup = ""
-# ArmarX.NavigationMemory.ArVizStorageName: Name of the ArViz storage
+# ArmarX.NavigationMemory.p.snapshotToLoad: No Description
# Attributes:
-# - Default: ArVizStorage
-# - Case sensitivity: yes
+# - Default: ./PriorKnowledgeData/navigation-graphs/R003
+# - Case sensitivity: no
# - Required: no
-# ArmarX.NavigationMemory.ArVizStorageName = ArVizStorage
-
-
-# ArmarX.NavigationMemory.ArVizTopicName: Name of the ArViz topic
-# Attributes:
-# - Default: ArVizTopic
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.NavigationMemory.ArVizTopicName = ArVizTopic
+ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/R003
-# ArmarX.NavigationMemory.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
-# - Default: false
+# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.NavigationMemory.EnableProfiling = false
+# ArmarX.RedirectStdout = true
-# ArmarX.NavigationMemory.MinimumLoggingLevel: Local logging level only for this component
+# 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: Undefined
+# - Default: 3000
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.NavigationMemory.MinimumLoggingLevel = Undefined
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.NavigationMemory.ObjectName: Name of IceGrid well-known object
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
-# - Default: ""
+# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.ObjectName = ""
+# ArmarX.SecondsStartupDelay = 0
-# ArmarX.NavigationMemory.RemoteGuiName: Name of the remote gui provider
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
# Attributes:
-# - Default: RemoteGuiProvider
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.RemoteGuiName = RemoteGuiProvider
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
-# ArmarX.NavigationMemory.mem.MemoryName: Name of this memory server.
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
# Attributes:
-# - Default: Navigation
+# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.MemoryName = Navigation
+# ArmarX.ThreadPoolSize = 1
-# ArmarX.NavigationMemory.mem.ltm.configuration:
+# 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: {}
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.configuration = {}
+# ArmarX.TopicSuffix = ""
-# ArmarX.NavigationMemory.mem.ltm.enabled:
+# 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.NavigationMemory.mem.ltm.enabled = false
+# ArmarX.UseTimeServer = false
-# ArmarX.NavigationMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.Verbosity: Global logging level for whole application
# Attributes:
-# - Default: true
+# - Default: Info
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.NavigationMemory.mns.MemoryNameSystemEnabled = true
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
-# ArmarX.NavigationMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.navigation_memory.ArVizStorageName: Name of the ArViz storage
# Attributes:
-# - Default: MemoryNameSystem
+# - Default: ArVizStorage
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.navigation_memory.ArVizStorageName = ArVizStorage
-# ArmarX.NavigationMemory.p.locationGraph.visuFrequency: Visualization frequeny of locations and graph edges [Hz].
+# ArmarX.navigation_memory.ArVizTopicName: Name of the ArViz topic
# Attributes:
-# - Default: 2
+# - Default: ArVizTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.p.locationGraph.visuFrequency = 2
+# ArmarX.navigation_memory.ArVizTopicName = ArVizTopic
-# ArmarX.NavigationMemory.p.locationGraph.visuGraphEdges: Enable visualization of navigation graph edges.
+# ArmarX.navigation_memory.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: true
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.NavigationMemory.p.locationGraph.visuGraphEdges = true
+# ArmarX.navigation_memory.EnableProfiling = false
-# ArmarX.NavigationMemory.p.locationGraph.visuLocation: Enable visualization of locations.
+# ArmarX.navigation_memory.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: true
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.NavigationMemory.p.locationGraph.visuLocation = true
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.navigation_memory.MinimumLoggingLevel = Undefined
-# ArmarX.NavigationMemory.p.snapshotToLoad: Memory snapshot to load at start up
-# (e.g. 'PriorKnowledgeData/navigation-graphs/snapshot').
+# ArmarX.navigation_memory.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/R003
+# ArmarX.navigation_memory.ObjectName = ""
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# ArmarX.navigation_memory.RemoteGuiName: Name of the remote gui provider
# Attributes:
-# - Default: true
+# - Default: RemoteGuiProvider
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
+# ArmarX.navigation_memory.RemoteGuiName = RemoteGuiProvider
-# 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)
+# ArmarX.navigation_memory.mem.MemoryName: Name of this memory server.
# Attributes:
-# - Default: 3000
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
+# ArmarX.navigation_memory.mem.MemoryName = Navigation
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# ArmarX.navigation_memory.mem.ltm.configuration:
# Attributes:
-# - Default: 0
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SecondsStartupDelay = 0
+# ArmarX.navigation_memory.mem.ltm.configuration = {}
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# ArmarX.navigation_memory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.StartDebuggerOnCrash = false
+# ArmarX.navigation_memory.mem.ltm.enabled = false
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# ArmarX.navigation_memory.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: 1
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ThreadPoolSize = 1
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.mns.MemoryNameSystemEnabled = true
-# 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.
+# ArmarX.navigation_memory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: ""
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.TopicSuffix = ""
+# ArmarX.navigation_memory.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# ArmarX.navigation_memory.p.locationGraph.visuFrequency: Visualization frequeny of locations and graph edges [Hz].
# Attributes:
-# - Default: false
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.p.locationGraph.visuFrequency = 2
+
+
+# ArmarX.navigation_memory.p.locationGraph.visuGraphEdges: Enable visualization of navigation graph edges.
+# Attributes:
+# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
+# ArmarX.navigation_memory.p.locationGraph.visuGraphEdges = true
-# ArmarX.Verbosity: Global logging level for whole application
+# ArmarX.navigation_memory.p.locationGraph.visuLocation: Enable visualization of locations.
# Attributes:
-# - Default: Info
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-ArmarX.Verbosity = Verbose
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.locationGraph.visuLocation = true
+
+
+# ArmarX.navigation_memory.p.snapshotToLoad: Memory snapshot to load at start up
+# (e.g. 'PriorKnowledgeData/navigation-graphs/snapshot').
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.navigation_memory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/R003
diff --git a/scenarios/PlatformNavigation/config/navigation_skill_provider.cfg b/scenarios/PlatformNavigation/config/navigation_skill_provider.cfg
index 3f23c22d1b03949146b5f948d7350624ca1dd258..e4209453dee3d1d833d2b3c3f171a6a4446dae42 100644
--- a/scenarios/PlatformNavigation/config/navigation_skill_provider.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_skill_provider.cfg
@@ -160,6 +160,22 @@
# ArmarX.Verbosity = Info
+# ArmarX.navigation_skill_provider.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_skill_provider.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.navigation_skill_provider.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_skill_provider.ArVizTopicName = ArVizTopic
+
+
# ArmarX.navigation_skill_provider.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index f6a497b25e20ac551dbcc98a5eb5fd28244e68df..51a93adba55c048f447354267f0d984bae923a71 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -92,298 +92,370 @@
# ArmarX.LoggingGroup = ""
-# ArmarX.Navigator.ArVizStorageName: Name of the ArViz storage
+# ArmarX.Navigator.ObjectName: No Description
# Attributes:
-# - Default: ArVizStorage
+# - Default: navigator
+# - Case sensitivity: no
+# - Required: no
+ArmarX.Navigator.ObjectName = navigator
+
+
+# ArmarX.Navigator.RobotUnitName: No Description
+# Attributes:
+# - Default: Armar6Unit
+# - Case sensitivity: no
+# - Required: no
+ArmarX.Navigator.RobotUnitName = Armar6Unit
+
+
+# ArmarX.Navigator.p.occupancy_grid.occopied_threshold: No Description
+# Attributes:
+# - Default: 0.8
+# - Case sensitivity: no
+# - Required: no
+ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.ArVizStorageName = ArVizStorage
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
-# ArmarX.Navigator.ArVizTopicName: Name of the ArViz topic
+# 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: ArVizTopic
+# - Default: 3000
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.ArVizTopicName = ArVizTopic
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.Navigator.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
-# - Default: false
+# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.Navigator.EnableProfiling = false
+# ArmarX.SecondsStartupDelay = 0
-# ArmarX.Navigator.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
# Attributes:
-# - Default: Undefined
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Navigator.MinimumLoggingLevel = Undefined
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
-# ArmarX.Navigator.ObjectMemoryName: Name of the object memory.
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
# Attributes:
-# - Default: ObjectMemory
+# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.ObjectMemoryName = ObjectMemory
+# ArmarX.ThreadPoolSize = 1
-# ArmarX.Navigator.ObjectName: Name of IceGrid well-known object
+# 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.Navigator.ObjectName = navigator
+# ArmarX.TopicSuffix = ""
-# ArmarX.Navigator.RobotUnitName: Name of the RobotUnit
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
+# - Default: false
# - Case sensitivity: yes
-# - Required: yes
-ArmarX.Navigator.RobotUnitName = Armar6Unit
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
-# ArmarX.Navigator.cmp.PlatformUnit: No Description
+# ArmarX.Verbosity: Global logging level for whole application
# Attributes:
-# - Default: Armar6PlatformUnit
-# - Case sensitivity: no
+# - Default: Info
+# - Case sensitivity: yes
# - Required: no
-ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
-# ArmarX.Navigator.cmp.RemoteGui: Ice object name of the `RemoteGui` component.
+# ArmarX.navigator.ArVizStorageName: Name of the ArViz storage
# Attributes:
-# - Default: RemoteGuiProvider
+# - Default: ArVizStorage
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.cmp.RemoteGui = RemoteGuiProvider
+# ArmarX.navigator.ArVizStorageName = ArVizStorage
-# ArmarX.Navigator.mem.nav.costmap.CoreSegment:
+# ArmarX.navigator.ArVizTopicName: Name of the ArViz topic
# Attributes:
-# - Default: Costmap
+# - Default: ArVizTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.costmap.CoreSegment = Costmap
+# ArmarX.navigator.ArVizTopicName = ArVizTopic
-# ArmarX.Navigator.mem.nav.costmap.Memory:
+# ArmarX.navigator.DebugObserverTopicName: Name of the topic the DebugObserver listens on
# Attributes:
-# - Default: Navigation
+# - Default: DebugObserver
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.costmap.Memory = Navigation
+# ArmarX.navigator.DebugObserverTopicName = DebugObserver
-# ArmarX.Navigator.mem.nav.costmap.Provider: Name of this provider
+# ArmarX.navigator.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: ""
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.costmap.Provider = ""
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigator.EnableProfiling = false
-# ArmarX.Navigator.mem.nav.events.CoreSegment:
+# ArmarX.navigator.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: Events
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.events.CoreSegment = Events
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.navigator.MinimumLoggingLevel = Undefined
-# ArmarX.Navigator.mem.nav.events.Memory:
+# ArmarX.navigator.ObjectMemoryName: Name of the object memory.
# Attributes:
-# - Default: Navigation
+# - Default: ObjectMemory
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.events.Memory = Navigation
+# ArmarX.navigator.ObjectMemoryName = ObjectMemory
-# ArmarX.Navigator.mem.nav.events.Provider: Name of this provider
+# ArmarX.navigator.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.events.Provider = ""
+# ArmarX.navigator.ObjectName = ""
-# ArmarX.Navigator.mem.nav.graph.CoreSegment:
+# ArmarX.navigator.RobotUnitName: Name of the RobotUnit
# Attributes:
-# - Default: Location
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.navigator.RobotUnitName = Armar6Unit
+
+
+# ArmarX.navigator.cmp.RemoteGui: Ice object name of the `RemoteGui` component.
+# Attributes:
+# - Default: RemoteGuiProvider
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.graph.CoreSegment = Location
+# ArmarX.navigator.cmp.RemoteGui = RemoteGuiProvider
-# ArmarX.Navigator.mem.nav.graph.Memory:
+# ArmarX.navigator.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.navigator.mem.nav.costmap.Memory:
# Attributes:
# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.graph.Memory = Navigation
+# ArmarX.navigator.mem.nav.costmap.Memory = Navigation
-# ArmarX.Navigator.mem.nav.param.CoreSegment:
+# ArmarX.navigator.mem.nav.events.CoreSegment:
# Attributes:
-# - Default: Parameterization
+# - Default: Events
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.param.CoreSegment = Parameterization
+# ArmarX.navigator.mem.nav.events.CoreSegment = Events
-# ArmarX.Navigator.mem.nav.param.Memory:
+# ArmarX.navigator.mem.nav.events.Memory:
# Attributes:
# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.param.Memory = Navigation
+# ArmarX.navigator.mem.nav.events.Memory = Navigation
-# ArmarX.Navigator.mem.nav.param.Provider: Name of this provider
+# ArmarX.navigator.mem.nav.events.Provider: Name of this provider
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.param.Provider = ""
+# ArmarX.navigator.mem.nav.events.Provider = ""
-# ArmarX.Navigator.mem.nav.stack_result.CoreSegment:
+# ArmarX.navigator.mem.nav.graph.CoreSegment:
# Attributes:
-# - Default: ""
+# - Default: Location
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.stack_result.CoreSegment = ""
+# ArmarX.navigator.mem.nav.graph.CoreSegment = Location
-# ArmarX.Navigator.mem.nav.stack_result.Memory:
+# ArmarX.navigator.mem.nav.graph.Memory:
# Attributes:
# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.stack_result.Memory = Navigation
+# ArmarX.navigator.mem.nav.graph.Memory = Navigation
-# ArmarX.Navigator.mem.nav.stack_result.Provider: Name of this provider
+# ArmarX.navigator.mem.nav.human.CoreSegment:
# Attributes:
-# - Default: ""
+# - Default: Human
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.stack_result.Provider = ""
+# ArmarX.navigator.mem.nav.human.CoreSegment = Human
-# ArmarX.Navigator.mem.robot_state.Memory:
+# ArmarX.navigator.mem.nav.human.Memory:
# Attributes:
-# - Default: RobotState
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.robot_state.Memory = RobotState
+# ArmarX.navigator.mem.nav.human.Memory = Navigation
-# ArmarX.Navigator.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# ArmarX.navigator.mem.nav.param.CoreSegment:
# Attributes:
-# - Default: Localization
+# - Default: Parameterization
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.robot_state.localizationSegment = Localization
+# ArmarX.navigator.mem.nav.param.CoreSegment = Parameterization
-# ArmarX.Navigator.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.navigator.mem.nav.param.Memory:
# Attributes:
-# - Default: true
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.Navigator.mns.MemoryNameSystemEnabled = true
+# ArmarX.navigator.mem.nav.param.Memory = Navigation
-# ArmarX.Navigator.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.navigator.mem.nav.param.Provider: Name of this provider
# Attributes:
-# - Default: MemoryNameSystem
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.navigator.mem.nav.param.Provider = ""
-# ArmarX.Navigator.p.occupancy_grid.occopied_threshold: Threshold for each cell to be considered occupied. Increase this value to reduce noise.
+# ArmarX.navigator.mem.nav.stack_result.CoreSegment:
# Attributes:
-# - Default: 0.550000012
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
+# ArmarX.navigator.mem.nav.stack_result.CoreSegment = ""
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# ArmarX.navigator.mem.nav.stack_result.Memory:
# Attributes:
-# - Default: true
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
+# ArmarX.navigator.mem.nav.stack_result.Memory = Navigation
-# 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)
+# ArmarX.navigator.mem.nav.stack_result.Provider: Name of this provider
# Attributes:
-# - Default: 3000
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
+# ArmarX.navigator.mem.nav.stack_result.Provider = ""
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# ArmarX.navigator.mem.robot_state.Memory:
# Attributes:
-# - Default: 0
+# - Default: RobotState
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SecondsStartupDelay = 0
+# ArmarX.navigator.mem.robot_state.Memory = RobotState
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# ArmarX.navigator.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
# Attributes:
-# - Default: false
+# - Default: Localization
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.StartDebuggerOnCrash = false
+# ArmarX.navigator.mem.robot_state.localizationSegment = Localization
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# ArmarX.navigator.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: 1
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ThreadPoolSize = 1
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigator.mns.MemoryNameSystemEnabled = true
-# 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.
+# ArmarX.navigator.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: ""
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.TopicSuffix = ""
+# ArmarX.navigator.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# ArmarX.navigator.p.disableExecutor: If the executor is disabled, the navigator will only plan the trajectory but won't execute it.
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
+# ArmarX.navigator.p.disableExecutor = false
-# ArmarX.Verbosity: Global logging level for whole application
+# ArmarX.navigator.p.occupancy_grid.occopied_threshold: Threshold for each cell to be considered occupied. Increase this value to reduce noise.
# Attributes:
-# - Default: Info
+# - Default: 0.550000012
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-ArmarX.Verbosity = Verbose
+# ArmarX.navigator.p.occupancy_grid.occopied_threshold = 0.550000012
+
+
+# ArmarX.navigator.p.scene.humanProviderName:
+# Attributes:
+# - Default: dynamic_scene_provider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.p.scene.humanProviderName = dynamic_scene_provider
+
+
+# ArmarX.navigator.p.scene.robotName:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.navigator.p.scene.robotName = Armar6
+
+
+# ArmarX.navigator.p.scene.staticCostmapName:
+# Attributes:
+# - Default: distance_to_obstacles
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.p.scene.staticCostmapName = distance_to_obstacles
+
+
+# ArmarX.navigator.p.scene.staticCostmapProviderName:
+# Attributes:
+# - Default: distance_to_obstacle_costmap_provider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.p.scene.staticCostmapProviderName = distance_to_obstacle_costmap_provider
diff --git a/source/armarx/navigation/CMakeLists.txt b/source/armarx/navigation/CMakeLists.txt
index 7f8a9d907424d7bb4a8649a59ebf4feb5a8d35ff..a0cd028bafa0f7f8100f3958376ddae447941a6e 100644
--- a/source/armarx/navigation/CMakeLists.txt
+++ b/source/armarx/navigation/CMakeLists.txt
@@ -3,6 +3,8 @@ add_subdirectory(core)
add_subdirectory(util)
add_subdirectory(conversions)
add_subdirectory(algorithms)
+add_subdirectory(human)
+add_subdirectory(dynamic_scene)
add_subdirectory(global_planning)
add_subdirectory(local_planning)
add_subdirectory(trajectory_control)
@@ -15,6 +17,7 @@ add_subdirectory(rooms)
add_subdirectory(memory)
add_subdirectory(server)
add_subdirectory(platform_controller)
+add_subdirectory(simulation)
add_subdirectory(skills)
# Components.
diff --git a/source/armarx/navigation/algorithms/CMakeLists.txt b/source/armarx/navigation/algorithms/CMakeLists.txt
index 23422ec563c43b6534270afe79d03b6f9703f167..595167566cacad7df265ae3bf048c9814f452a4f 100644
--- a/source/armarx/navigation/algorithms/CMakeLists.txt
+++ b/source/armarx/navigation/algorithms/CMakeLists.txt
@@ -1,6 +1,7 @@
armarx_add_aron_library(algorithms_aron
ARON_FILES
aron/Costmap.xml
+ aron/ShortestPathFasterAlgorithmParams.xml
)
armarx_add_library(algorithms
@@ -13,6 +14,7 @@ armarx_add_library(algorithms
DEPENDENCIES_PRIVATE
OpenMP::OpenMP_CXX
range-v3::range-v3
+ ArViz # RobotAPI
DEPENDENCIES_LEGACY
OpenCV
SOURCES
@@ -32,6 +34,7 @@ armarx_add_library(algorithms
# smoothing
smoothing/ChainApproximation.cpp
smoothing/CircularPathSmoothing.cpp
+ visualization.cpp
HEADERS
types.h
algorithms.h
@@ -50,6 +53,7 @@ armarx_add_library(algorithms
# smoothing
smoothing/ChainApproximation.h
smoothing/CircularPathSmoothing.h
+ visualization.h
)
diff --git a/source/armarx/navigation/algorithms/Costmap.cpp b/source/armarx/navigation/algorithms/Costmap.cpp
index d31dceb15c13c4991ce01c9b6f7ee1ed428d30be..862dc263557a2ff76706a3554db6a54ab1f4c09a 100644
--- a/source/armarx/navigation/algorithms/Costmap.cpp
+++ b/source/armarx/navigation/algorithms/Costmap.cpp
@@ -194,13 +194,11 @@ namespace armarx::navigation::algorithms
bool
Costmap::add(const Costmap& other, const float weight)
{
- ARMARX_INFO << "1";
// ensure that both grid and mask are the same size
ARMARX_TRACE;
validateSizes();
other.validateSizes();
- ARMARX_INFO << "1";
const auto startIdx = toVertex(other.sceneBounds.min);
@@ -210,7 +208,6 @@ namespace armarx::navigation::algorithms
// In this case, only add the part to this costmap that is possible.
const int rows = std::min(other.grid.rows(), grid.rows() - startIdx.index.x());
const int cols = std::min(other.grid.cols(), grid.cols() - startIdx.index.y());
- ARMARX_INFO << "1";
ARMARX_VERBOSE << "Adding other grid to region (" << startIdx.index.x() << ", "
<< startIdx.index.y() << "), "
@@ -218,7 +215,6 @@ namespace armarx::navigation::algorithms
<< ")";
ARMARX_TRACE;
- ARMARX_INFO << "1";
// add the costs of the other mask to this one by a weighting factor
grid.block(startIdx.index.x(), startIdx.index.y(), rows, cols).array() +=
@@ -366,7 +362,7 @@ namespace armarx::navigation::algorithms
std::optional<float>
Costmap::value(const Position& position) const
{
- ARMARX_INFO << "value ...";
+ ARMARX_DEBUG << "value ...";
const auto v = toVertex(position);
return value(v.index);
@@ -381,7 +377,7 @@ namespace armarx::navigation::algorithms
const std::vector<float> costmapWeights(weights.begin(), weights.end() - 1);
ARMARX_CHECK_EQUAL(costmapWeights.size(), costmaps.size());
- ARMARX_INFO << "Merging into with weights " << weights;
+ ARMARX_VERBOSE << "Merging into with weights " << weights;
Costmap mergedCostmap = *this;
diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index 6f452b7dcc5e0fb3f8265f344c54b6b3701cd13b..e7f4fc0c9ab931e302e2a2b0c491cd029efaff9c 100644
--- a/source/armarx/navigation/algorithms/Costmap.h
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -16,6 +16,7 @@ namespace armarx::navigation::algorithms
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
friend class CostmapBuilder;
+ friend class SocialCostmapBuilder;
struct Parameters
{
@@ -68,8 +69,10 @@ namespace armarx::navigation::algorithms
centerPose() const
{
const Eigen::Vector2f costmap_P_center{
- (getLocalSceneBounds().max.x() - getLocalSceneBounds().min.x())/2 + getLocalSceneBounds().min.x(),
- (getLocalSceneBounds().max.y() - getLocalSceneBounds().min.y())/2 + getLocalSceneBounds().min.y()};
+ (getLocalSceneBounds().max.x() - getLocalSceneBounds().min.x()) / 2 +
+ getLocalSceneBounds().min.x(),
+ (getLocalSceneBounds().max.y() - getLocalSceneBounds().min.y()) / 2 +
+ getLocalSceneBounds().min.y()};
return global_T_costmap * Eigen::Translation2f(costmap_P_center);
}
diff --git a/source/armarx/navigation/algorithms/CostmapBuilder.cpp b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
index 9c764b22a1b6d95db07de306a12e93a68e101f82..99460765078f163de4224074a9de2b1b78ac9a3e 100644
--- a/source/armarx/navigation/algorithms/CostmapBuilder.cpp
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
@@ -61,13 +61,13 @@ namespace armarx::navigation::algorithms
const auto sceneBounds = computeSceneBounds(obstacles);
const auto grid = createUniformGrid(sceneBounds, parameters);
- ARMARX_INFO << "Creating grid";
+ ARMARX_VERBOSE << "Creating grid";
Costmap costmap(grid, parameters, sceneBounds);
- ARMARX_INFO << "Filling grid with size (" << costmap.getGrid().rows() << "/"
- << costmap.getGrid().cols() << ")";
+ ARMARX_VERBOSE << "Filling grid with size (" << costmap.getGrid().rows() << "/"
+ << costmap.getGrid().cols() << ")";
fillGridCosts(costmap);
- ARMARX_INFO << "Filled grid";
+ ARMARX_VERBOSE << "Filled grid";
initializeMask(costmap);
@@ -79,7 +79,8 @@ namespace armarx::navigation::algorithms
{
costmap.mask = costmap.grid.array() > 0.F;
- ARMARX_INFO << "Initializing mask: Fraction of valid elements: " << costmap.mask->cast<float>().sum() / costmap.mask->size();
+ ARMARX_VERBOSE << "Initializing mask: Fraction of valid elements: "
+ << costmap.mask->cast<float>().sum() / costmap.mask->size();
}
Eigen::MatrixXf
@@ -88,15 +89,15 @@ namespace armarx::navigation::algorithms
{
ARMARX_TRACE;
- ARMARX_INFO << "Scene bounds are " << sceneBounds.min << " and " << sceneBounds.max;
+ ARMARX_VERBOSE << "Scene bounds are " << sceneBounds.min << " and " << sceneBounds.max;
//+1 for explicit rounding up
size_t c_x = (sceneBounds.max.x() - sceneBounds.min.x()) / parameters.cellSize + 1;
size_t c_y = (sceneBounds.max.y() - sceneBounds.min.y()) / parameters.cellSize + 1;
- ARMARX_INFO << "Grid size: " << c_x << ", " << c_y;
+ ARMARX_VERBOSE << "Grid size: " << c_x << ", " << c_y;
- ARMARX_INFO << "Resetting grid";
+ ARMARX_VERBOSE << "Resetting grid";
Eigen::MatrixXf grid(c_x, c_y);
grid.setZero();
diff --git a/source/armarx/navigation/algorithms/aron/Costmap.xml b/source/armarx/navigation/algorithms/aron/Costmap.xml
index a52b29c3d80fdd9fbf0f2dfd4f59e41623fc8d02..8719e14dd49ff024d68aee964e7c3c675e09ebf4 100644
--- a/source/armarx/navigation/algorithms/aron/Costmap.xml
+++ b/source/armarx/navigation/algorithms/aron/Costmap.xml
@@ -1,8 +1,6 @@
<!--Some fancy comment -->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- </CodeIncludes>
<AronIncludes>
</AronIncludes>
diff --git a/source/armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.xml b/source/armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..91f0b6a694ba5d9cd99522635fd123bb42de65ae
--- /dev/null
+++ b/source/armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ </AronIncludes>
+
+ <GenerateTypes>
+ <Object name='armarx::navigation::algorithms::arondto::ShortestPathFasterAlgorithmParams'>
+ <ObjectChild key='obstacleDistanceCosts'>
+ <bool />
+ </ObjectChild>
+ <ObjectChild key='obstacleMaxDistance'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='obstacleDistanceWeight'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='obstacleCostExponent'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/algorithms/aron_conversions.cpp b/source/armarx/navigation/algorithms/aron_conversions.cpp
index 28258339ce148c4205ee629e843d6e12d0d91aee..ae913a44b0bb63329e9054bb0656184071c9331d 100644
--- a/source/armarx/navigation/algorithms/aron_conversions.cpp
+++ b/source/armarx/navigation/algorithms/aron_conversions.cpp
@@ -2,10 +2,11 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.aron.generated.h>
#include <armarx/navigation/conversions/eigen.h>
@@ -70,7 +71,7 @@ namespace armarx::navigation::algorithms
ARMARX_DEBUG << "Converting grid";
const auto gridNavigator =
aron::data::NDArray::DynamicCast(entityInstance.data()->getElement("grid"));
-
+
ARMARX_CHECK_NOT_NULL(gridNavigator);
Costmap::Grid grid =
@@ -95,5 +96,26 @@ namespace armarx::navigation::algorithms
return {grid, parameters, sceneBounds, mask};
}
+ void
+ toAron(arondto::ShortestPathFasterAlgorithmParams& dto,
+ const spfa::ShortestPathFasterAlgorithm::Parameters& bo)
+ {
+ dto.obstacleDistanceCosts = bo.obstacleDistanceCosts;
+ dto.obstacleMaxDistance = bo.obstacleMaxDistance;
+ dto.obstacleDistanceWeight = bo.obstacleDistanceWeight;
+ dto.obstacleCostExponent = bo.obstacleCostExponent;
+ }
+
+
+ void
+ fromAron(const arondto::ShortestPathFasterAlgorithmParams& dto,
+ spfa::ShortestPathFasterAlgorithm::Parameters& bo)
+ {
+ bo.obstacleDistanceCosts = dto.obstacleDistanceCosts;
+ bo.obstacleMaxDistance = dto.obstacleMaxDistance;
+ bo.obstacleDistanceWeight = dto.obstacleDistanceWeight;
+ bo.obstacleCostExponent = dto.obstacleCostExponent;
+ }
+
} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/aron_conversions.h b/source/armarx/navigation/algorithms/aron_conversions.h
index 4c91b8330c4f53b4303db407abe53c9fa8f2b5f1..965857a26b4b2017c249517e7dc6ac7372dcfdda 100644
--- a/source/armarx/navigation/algorithms/aron_conversions.h
+++ b/source/armarx/navigation/algorithms/aron_conversions.h
@@ -22,6 +22,7 @@
#pragma once
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
@@ -43,5 +44,14 @@ namespace armarx::navigation::algorithms
// Costmap does not provide a default c'tor
Costmap fromAron(const armem::wm::EntityInstance& entityInstance);
+ namespace arondto
+ {
+ class ShortestPathFasterAlgorithmParams;
+ }
+
+ void toAron(arondto::ShortestPathFasterAlgorithmParams& dto, const spfa::ShortestPathFasterAlgorithm::Parameters& bo);
+
+ void fromAron(const arondto::ShortestPathFasterAlgorithmParams& dto, spfa::ShortestPathFasterAlgorithm::Parameters& bo);
+
} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
index 5d3c7ad0d354e7fc9323f17a86cd6bec6fc313d5..9ce4afe6a7a893d3b86a0438a14bb48c289bcd12 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
@@ -63,7 +63,7 @@ namespace armarx::navigation::algorithm::astar
const size_t cols = costmap.getGrid().cols();
// create the grid with the correct size
- ARMARX_INFO << "Creating grid";
+ ARMARX_VERBOSE << "Creating grid";
for (size_t r = 0; r < rows; r++)
{
grid.push_back(std::vector<NodePtr>(cols));
@@ -81,7 +81,7 @@ namespace armarx::navigation::algorithm::astar
}
}
- ARMARX_INFO << "Creating graph";
+ ARMARX_VERBOSE << "Creating graph";
// Init successors
for (size_t r = 0; r < rows; r++)
@@ -118,7 +118,7 @@ namespace armarx::navigation::algorithm::astar
}
}
- ARMARX_INFO << "Done.";
+ ARMARX_VERBOSE << "Done.";
}
bool
diff --git a/source/armarx/navigation/algorithms/astar/Planner2D.cpp b/source/armarx/navigation/algorithms/astar/Planner2D.cpp
index 05e3e9be6f5486f1de7ba8cf50db84d564d2255a..73f1219a3c4fd8a7ea39fd5e6cc253a4d9b698bb 100644
--- a/source/armarx/navigation/algorithms/astar/Planner2D.cpp
+++ b/source/armarx/navigation/algorithms/astar/Planner2D.cpp
@@ -4,6 +4,7 @@
#include <cmath>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include "ArmarXCore/core/logging/Logging.h"
namespace armarx::navigation::algorithm::astar
{
@@ -71,7 +72,7 @@ namespace armarx::navigation::algorithm::astar
{
if (!hasParameter(s))
{
- std::cout << "Warning, parameter " << s << " not set, returning 0" << std::endl;
+ ARMARX_WARNING << "Parameter " << s << " not set, returning 0";
return 0.0f;
}
return parameters[s];
diff --git a/source/armarx/navigation/algorithms/persistence.cpp b/source/armarx/navigation/algorithms/persistence.cpp
index 7b55a46efc652ede1c85aa5012c574d33ca7077f..efcef6630ab736b4623a591e6385ec62288b020e 100644
--- a/source/armarx/navigation/algorithms/persistence.cpp
+++ b/source/armarx/navigation/algorithms/persistence.cpp
@@ -48,20 +48,22 @@ namespace armarx::navigation::algorithms
load(const std::filesystem::path& directory)
{
// load
- std::ifstream ifs(directory / "costmap.json");
+ const auto filename = directory / "costmap.json";
+ std::ifstream ifs(filename);
+ ARMARX_VERBOSE << "Loading costmap info " << filename.string();
const nlohmann::json j = nlohmann::json::parse(ifs);
// params
- const auto& jParam = j["params"];
+ const auto& jParam = j.at("params");
- const Costmap::Parameters params{.binaryGrid = jParam["binary_grid"],
- .cellSize = jParam["cell_size"]};
+ const Costmap::Parameters params{.binaryGrid = jParam.at("binary_grid"),
+ .cellSize = jParam.at("cell_size")};
// scene bounds
- const auto& jSceneBounds = j["scene_bounds"];
+ const auto& jSceneBounds = j.at("scene_bounds");
- const std::vector<float> boundsMin = jSceneBounds["min"];
- const std::vector<float> boundsMax = jSceneBounds["max"];
+ const std::vector<float> boundsMin = jSceneBounds.at("min");
+ const std::vector<float> boundsMax = jSceneBounds.at("max");
ARMARX_CHECK_EQUAL(boundsMin.size(), 2);
ARMARX_CHECK_EQUAL(boundsMax.size(), 2);
@@ -71,7 +73,7 @@ namespace armarx::navigation::algorithms
// grid
- const std::string gridFilename = j["grid_filename"];
+ const std::string gridFilename = j.at("grid_filename");
cv::Mat gridMat = cv::imread((directory / gridFilename).string(), cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);
Eigen::MatrixXf grid;
@@ -79,9 +81,9 @@ namespace armarx::navigation::algorithms
// mask, if any
std::optional<Costmap::Mask> optMask;
- if (not j["mask_filename"].empty())
+ if (not j.at("mask_filename").empty())
{
- const std::string maskFilename = j["grid_filename"];
+ const std::string maskFilename = j.at("grid_filename");
cv::Mat maskMat = cv::imread((directory / maskFilename).string(), cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);
Eigen::Matrix<std::uint8_t, Eigen::Dynamic, Eigen::Dynamic> mask;
@@ -103,7 +105,7 @@ namespace armarx::navigation::algorithms
if (not std::filesystem::exists(directory))
{
- ARMARX_INFO << "Creating directory `" << directory.string() << "`";
+ ARMARX_VERBOSE << "Creating directory `" << directory.string() << "`";
std::filesystem::create_directories(directory);
}
@@ -111,17 +113,17 @@ namespace armarx::navigation::algorithms
// params
{
- auto& jParam = j["params"];
- jParam["binary_grid"] = costmap.params().binaryGrid;
- jParam["cell_size"] = costmap.params().cellSize;
+ auto& jParam = j.at("params");
+ jParam.at("binary_grid") = costmap.params().binaryGrid;
+ jParam.at("cell_size") = costmap.params().cellSize;
}
// scene bounds
{
- auto& jSceneBounds = j["scene_bounds"];
- jSceneBounds["min"] = std::vector<float>{costmap.getLocalSceneBounds().min.x(),
+ auto& jSceneBounds = j.at("scene_bounds");
+ jSceneBounds.at("min") = std::vector<float>{costmap.getLocalSceneBounds().min.x(),
costmap.getLocalSceneBounds().min.y()};
- jSceneBounds["max"] = std::vector<float>{costmap.getLocalSceneBounds().max.x(),
+ jSceneBounds.at("max") = std::vector<float>{costmap.getLocalSceneBounds().max.x(),
costmap.getLocalSceneBounds().max.y()};
}
@@ -135,7 +137,7 @@ namespace armarx::navigation::algorithms
const std::string gridFilename = "grid.exr";
cv::imwrite((directory / gridFilename).string(), grid);
- j["grid_filename"] = gridFilename;
+ j.at("grid_filename") = gridFilename;
// for debugging purpose, also save a png image
const std::string gridDebuggingFilename = "grid.png";
@@ -160,7 +162,7 @@ namespace armarx::navigation::algorithms
const std::string maskFilename = "mask.ppm";
cv::imwrite((directory / maskFilename).string(), mask);
- j["mask_filename"] = maskFilename;
+ j.at("mask_filename") = maskFilename;
}
}
diff --git a/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.cpp b/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.cpp
index 083dcfcc24daec70aa6573b5cba6191ccccc9a04..3da33720e6eca9472caeebe4459cfef65a29fd2f 100644
--- a/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.cpp
+++ b/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.cpp
@@ -68,10 +68,10 @@ namespace armarx::navigation::algorithm
return points;
}
- core::Trajectory
- CircularPathSmoothing::smooth(const core::Trajectory& trajectory)
+ core::GlobalTrajectory
+ CircularPathSmoothing::smooth(const core::GlobalTrajectory& trajectory)
{
- core::TrajectoryPoints points;
+ core::GlobalTrajectoryPoints points;
const auto& p = trajectory.points();
points.push_back(p.front());
@@ -107,7 +107,7 @@ namespace armarx::navigation::algorithm
const Eigen::Vector2f prePos = at + Eigen::Vector2f(prev - at).normalized() * l;
const Eigen::Vector2f postPos = at + Eigen::Vector2f(next - at).normalized() * l;
- const auto tpProj = [&](const Eigen::Vector2f& pos) -> core::TrajectoryPoint
+ const auto tpProj = [&](const Eigen::Vector2f& pos) -> core::GlobalTrajectoryPoint
{
const auto proj =
trajectory.getProjection(navigation::conv::to3D(pos),
diff --git a/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.h b/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.h
index 4e4d45bf6089437901ed353bb7e5ced65180ada1..a36cf05dc1a876855921b33f9c99df8de4470ac1 100644
--- a/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.h
+++ b/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.h
@@ -41,7 +41,7 @@ namespace armarx::navigation::algorithm
/// circular path smoothing
Points smooth(const Points& p);
- core::Trajectory smooth(const core::Trajectory& trajectory);
+ core::GlobalTrajectory smooth(const core::GlobalTrajectory& trajectory);
};
} // namespace armarx::navigation::algorithm
diff --git a/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp b/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
index 6a0db3ebbad1f031938a2272532af836f068c87f..98a6934af3592c828b76ca66d3c1b80be8d7ca70 100644
--- a/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
+++ b/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
@@ -155,11 +155,11 @@ BOOST_AUTO_TEST_CASE(testGrid)
std::ifstream fs(testExrMetaFilename);
auto j = nlohmann::json::parse(fs);
- const float cellSize = j["cell_size"]; // [mm]
+ const float cellSize = j.at("cell_size"); // [mm]
BOOST_REQUIRE_GT(cellSize, 0);
- std::vector<float> sceneBoundsMinV(j["scene_bounds"]["min"]);
- std::vector<float> sceneBoundsMaxV(j["scene_bounds"]["max"]);
+ const std::vector<float> sceneBoundsMinV(j.at("scene_bounds").at("min"));
+ const std::vector<float> sceneBoundsMaxV(j.at("scene_bounds").at("max"));
BOOST_REQUIRE_EQUAL(sceneBoundsMinV.size(), 2);
BOOST_REQUIRE_EQUAL(sceneBoundsMaxV.size(), 2);
@@ -209,11 +209,11 @@ BOOST_AUTO_TEST_CASE(testSPFAPlanWObstacleDistance)
std::ifstream fs(testExrMetaFilename);
const auto j = nlohmann::json::parse(fs);
- const float cellSize = j["cell_size"]; // [mm]
+ const float cellSize = j.at("cell_size"); // [mm]
BOOST_REQUIRE_GT(cellSize, 0);
- std::vector<float> sceneBoundsMinV(j["scene_bounds"]["min"]);
- std::vector<float> sceneBoundsMaxV(j["scene_bounds"]["max"]);
+ std::vector<float> sceneBoundsMinV(j.at("scene_bounds").at("min"));
+ std::vector<float> sceneBoundsMaxV(j.at("scene_bounds").at("max"));
BOOST_REQUIRE_EQUAL(sceneBoundsMinV.size(), 2);
BOOST_REQUIRE_EQUAL(sceneBoundsMaxV.size(), 2);
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/algorithms/visualization.cpp b/source/armarx/navigation/algorithms/visualization.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a28cbeb21e97d3331a97844c390aa2d9a27f0cda
--- /dev/null
+++ b/source/armarx/navigation/algorithms/visualization.cpp
@@ -0,0 +1,62 @@
+#include "visualization.h"
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/color/cmaps/colormaps.h>
+
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+#include <RobotAPI/components/ArViz/Client/elements/Mesh.h>
+
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/conversions/eigen.h>
+
+
+namespace armarx::navigation::algorithms
+{
+
+ void
+ visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name)
+ {
+ const auto cmap = simox::color::cmaps::viridis();
+ const float vmax = costmap.getGrid().array().maxCoeff();
+
+ const auto asColor = [&cmap, &vmax](const float distance,
+ const bool isValid) -> viz::data::Color
+ {
+ const auto color = cmap.at(distance, 0.F, vmax);
+ const Ice::Byte alpha = isValid ? color.a : 0;
+ return {alpha, color.r, color.g, color.b};
+ };
+
+ const std::int64_t cols = costmap.getGrid().cols();
+ const std::int64_t rows = costmap.getGrid().rows();
+
+ auto mesh = viz::Mesh(name);
+
+ std::vector<std::vector<Eigen::Vector3f>> vertices;
+ std::vector<std::vector<viz::data::Color>> colors;
+
+ for (int r = 0; r < rows; r++)
+ {
+ auto& verticesRow = vertices.emplace_back(cols);
+ auto& colorsRow = colors.emplace_back(cols);
+
+ for (int c = 0; c < cols; c++)
+ {
+ verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
+
+ bool isValid =
+ costmap.getMask().has_value() ? costmap.getMask().value()(r, c) : true;
+ colorsRow.at(c) = asColor(costmap.getGrid()(r, c), isValid);
+ }
+ }
+
+ mesh.grid2D(vertices, colors);
+
+ layer.add(mesh);
+ }
+
+
+} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/visualization.h b/source/armarx/navigation/algorithms/visualization.h
new file mode 100644
index 0000000000000000000000000000000000000000..df077495b1b28de746d5952992e1edc7dfd028ae
--- /dev/null
+++ b/source/armarx/navigation/algorithms/visualization.h
@@ -0,0 +1,38 @@
+/**
+ * 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 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+namespace armarx::viz
+{
+ class Layer;
+}
+
+
+namespace armarx::navigation::algorithms
+{
+ class Costmap;
+
+ void visualize(const Costmap& costmap, viz::Layer& layer, const std::string& name);
+
+
+} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/client/ComponentPlugin.cpp b/source/armarx/navigation/client/ComponentPlugin.cpp
index b047700cea8530733351fb45c55931b4d0ae9bfc..2ecd8fce8a8826cb4d7afca09fddc7d3b66cf898 100644
--- a/source/armarx/navigation/client/ComponentPlugin.cpp
+++ b/source/armarx/navigation/client/ComponentPlugin.cpp
@@ -17,93 +17,99 @@
// ComponentPlugin
-armarx::navigation::client::ComponentPlugin::~ComponentPlugin() = default;
-
-void
-armarx::navigation::client::ComponentPlugin::postCreatePropertyDefinitions(
- armarx::PropertyDefinitionsPtr& properties)
+namespace armarx::navigation::client
{
- if (not properties->hasDefinition(PROPERTY_NAME))
- {
- properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME, "navigator", "Name of the Navigator");
- }
-}
-void
-armarx::navigation::client::ComponentPlugin::preOnInitComponent()
-{
- ARMARX_TRACE;
- parent<armarx::Component>().usingProxyFromProperty(PROPERTY_NAME);
-}
+ ComponentPlugin::~ComponentPlugin() = default;
-void
-armarx::navigation::client::ComponentPlugin::preOnConnectComponent()
-{
- ARMARX_TRACE;
- parent<armarx::Component>().getProxyFromProperty(navigatorPrx, PROPERTY_NAME);
+ void
+ ComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (not properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME, "navigator", "Name of the Navigator");
+ }
+ }
- ARMARX_TRACE;
- const std::string componentName = parent().getName();
+ void
+ ComponentPlugin::preOnInitComponent()
+ {
+ ARMARX_TRACE;
+ parent<armarx::Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
- ARMARX_CHECK_NOT_NULL(navigatorPrx) << "Navigator proxy is null!";
- iceNavigator.setNavigatorComponent(navigatorPrx);
-}
+ void
+ ComponentPlugin::preOnConnectComponent()
+ {
+ ARMARX_TRACE;
+ parent<armarx::Component>().getProxyFromProperty(navigatorPrx, PROPERTY_NAME);
-void
-armarx::navigation::client::ComponentPlugin::configureNavigator(
- const client::NavigationStackConfig& stackConfig,
- const std::string& configId)
-{
- ARMARX_TRACE;
+ ARMARX_TRACE;
+ const std::string componentName = parent().getName();
- // ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
+ ARMARX_CHECK_NOT_NULL(navigatorPrx) << "Navigator proxy is null!";
+ iceNavigator.setNavigatorComponent(navigatorPrx);
+ }
- eventHandler = [&]() -> std::unique_ptr<SimpleEventHandler>
+ void
+ ComponentPlugin::configureNavigator(const client::NavigationStackConfig& stackConfig,
+ const std::string& configId)
{
- if (parentDerives<armarx::armem::client::plugins::ListeningPluginUser>())
- {
- ARMARX_INFO << "Using memory event callbacks.";
- // must(!!) use a reference here: otherwise the mns events won't work
- auto& memoryNameSystem =
- parent<armarx::armem::client::plugins::ListeningPluginUser>().memoryNameSystem();
- return std::make_unique<MemorySubscriber>(configId, memoryNameSystem);
- }
+ ARMARX_TRACE;
- ARMARX_INFO << "Cannot use memory event callbacks as this component is not derived from "
- "`armarx::armem::client::plugins::ListeningPluginUser`";
+ // ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
- return std::make_unique<SimpleEventHandler>();
- }();
-
- iceNavigator.createConfig(stackConfig, configId);
+ eventHandler = [&]() -> std::unique_ptr<SimpleEventHandler>
+ {
+ ARMARX_TRACE;
+ if (parentDerives<armarx::armem::client::plugins::ListeningPluginUser>())
+ {
+ ARMARX_TRACE;
+ ARMARX_INFO << "Using memory event callbacks.";
+ // must(!!) use a reference here: otherwise the mns events won't work
+ auto& memoryNameSystem =
+ parent<armarx::armem::client::plugins::ListeningPluginUser>()
+ .memoryNameSystem();
+ return std::make_unique<MemorySubscriber>(configId, memoryNameSystem);
+ }
+
+ ARMARX_INFO
+ << "Cannot use memory event callbacks as this component is not derived from "
+ "`armarx::armem::client::plugins::ListeningPluginUser`";
+
+ return std::make_unique<SimpleEventHandler>();
+ }();
+
+ iceNavigator.createConfig(stackConfig, configId);
+
+ navigator = std::make_unique<Navigator>(Navigator::InjectedServices{
+ .navigator = &iceNavigator, .subscriber = eventHandler.get()});
+ }
- navigator = std::make_unique<Navigator>(
- Navigator::InjectedServices{.navigator = &iceNavigator, .subscriber = eventHandler.get()});
-}
+ // ComponentPluginUser
-// ComponentPluginUser
+ ComponentPluginUser::ComponentPluginUser()
+ {
+ ARMARX_TRACE;
+ addPlugin(plugin);
+ }
-armarx::navigation::client::ComponentPluginUser::ComponentPluginUser()
-{
- ARMARX_TRACE;
- addPlugin(plugin);
-}
+ void
+ ComponentPluginUser::configureNavigator(const client::NavigationStackConfig& stackConfig)
+ {
+ ARMARX_TRACE;
+ plugin->configureNavigator(stackConfig, getName());
+ }
-void
-armarx::navigation::client::ComponentPluginUser::configureNavigator(
- const client::NavigationStackConfig& stackConfig)
-{
- ARMARX_TRACE;
- plugin->configureNavigator(stackConfig, getName());
-}
+ Navigator&
+ ComponentPluginUser::getNavigator()
+ {
+ ARMARX_CHECK_NOT_NULL(plugin->navigator)
+ << "You need to call `configureNavigator()` before you can access the navigator!";
+ return *plugin->navigator;
+ }
-armarx::navigation::client::Navigator&
-armarx::navigation::client::ComponentPluginUser::getNavigator()
-{
- ARMARX_CHECK_NOT_NULL(plugin->navigator)
- << "You need to call `configureNavigator()` before you can access the navigator!";
- return *plugin->navigator;
-}
+ ComponentPluginUser::~ComponentPluginUser() = default;
-armarx::navigation::client::ComponentPluginUser::~ComponentPluginUser() = default;
+} // namespace armarx::navigation::client
diff --git a/source/armarx/navigation/client/NavigationStackConfig.cpp b/source/armarx/navigation/client/NavigationStackConfig.cpp
index 5e23bb7f830609164db6f499719100290b0d67ed..cb70009daf32c86aef5c44f9787395c800c04d2b 100644
--- a/source/armarx/navigation/client/NavigationStackConfig.cpp
+++ b/source/armarx/navigation/client/NavigationStackConfig.cpp
@@ -74,12 +74,12 @@ namespace armarx::navigation::client
}
NavigationStackConfig&
- NavigationStackConfig::localPlanner(const loc_plan::LocalPlannerParams& params)
+ NavigationStackConfig::localPlanner(const local_planning::LocalPlannerParams& params)
{
aron::data::DictPtr element(new aron::data::Dict);
element->addElement(core::NAME_KEY,
std::make_shared<aron::data::String>(
- loc_plan::AlgorithmNames.to_name(params.algorithm())));
+ local_planning::AlgorithmNames.to_name(params.algorithm())));
element->addElement(core::PARAMS_KEY, params.toAron());
dict.addElement(core::StackLayerNames.to_name(core::StackLayer::LocalPlanner), element);
@@ -88,12 +88,12 @@ namespace armarx::navigation::client
}
NavigationStackConfig&
- NavigationStackConfig::trajectoryController(const traj_ctrl::TrajectoryControllerParams& params)
+ NavigationStackConfig::trajectoryController(const traj_ctrl::local::TrajectoryControllerParams& params)
{
aron::data::DictPtr element(new aron::data::Dict);
element->addElement(core::NAME_KEY,
std::make_shared<aron::data::String>(
- traj_ctrl::AlgorithmNames.to_name(params.algorithm())));
+ traj_ctrl::local::AlgorithmNames.to_name(params.algorithm())));
element->addElement(core::PARAMS_KEY, params.toAron());
dict.addElement(core::StackLayerNames.to_name(core::StackLayer::TrajectoryController),
diff --git a/source/armarx/navigation/client/NavigationStackConfig.h b/source/armarx/navigation/client/NavigationStackConfig.h
index 2e3d690105ae0c898915b63c8e04c1d7cdbf3f63..8125abe58b8bac2406e5f3a8717b4dbbf01b65dc 100644
--- a/source/armarx/navigation/client/NavigationStackConfig.h
+++ b/source/armarx/navigation/client/NavigationStackConfig.h
@@ -33,7 +33,8 @@
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/safety_control/SafetyController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::client
@@ -61,10 +62,10 @@ namespace armarx::navigation::client
NavigationStackConfig& globalPlanner(const global_planning::GlobalPlannerParams& params);
- NavigationStackConfig& localPlanner(const loc_plan::LocalPlannerParams& params);
+ NavigationStackConfig& localPlanner(const local_planning::LocalPlannerParams& params);
NavigationStackConfig&
- trajectoryController(const traj_ctrl::TrajectoryControllerParams& params);
+ trajectoryController(const traj_ctrl::local::TrajectoryControllerParams& params);
NavigationStackConfig& safetyController(const safe_ctrl::SafetyControllerParams& params);
diff --git a/source/armarx/navigation/client/Navigator.cpp b/source/armarx/navigation/client/Navigator.cpp
index 73ab51bcc842b86a07374cb7815b921d578428e4..442aac1d290395b5d70cbef01ec080ecc9c77699 100644
--- a/source/armarx/navigation/client/Navigator.cpp
+++ b/source/armarx/navigation/client/Navigator.cpp
@@ -72,6 +72,13 @@ namespace armarx::navigation::client
}
srv.navigator->moveTo(path, frame);
}
+
+ void Navigator::update(const std::vector<core::Pose>& waypoints, core::NavigationFrame frame)
+ {
+ ARMARX_TRACE;;
+ ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
+ srv.navigator->update(waypoints, frame);
+ }
void
@@ -88,6 +95,20 @@ namespace armarx::navigation::client
}
srv.navigator->moveTowards(direction, frame);
}
+
+ void Navigator::moveToLocation(const std::string& location)
+ {
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
+
+ {
+ // TODO: This still leads to a race condition, if extern a stop event is generated before moveTo but arrives
+ // after the event is reset
+ std::scoped_lock const l{stoppedInfo.m};
+ stoppedInfo.event.reset();
+ }
+ srv.navigator->moveToLocation(location);
+ }
void
diff --git a/source/armarx/navigation/client/Navigator.h b/source/armarx/navigation/client/Navigator.h
index d116a1de824c1a9903bb695055dc7559307c22c4..8b84f9556f8b092290743dcfa6924932d350b85c 100644
--- a/source/armarx/navigation/client/Navigator.h
+++ b/source/armarx/navigation/client/Navigator.h
@@ -132,6 +132,10 @@ namespace armarx::navigation::client
void moveTowards(const core::Direction& direction, core::NavigationFrame frame);
+ void moveToLocation(const std::string& location);
+
+ void update(const std::vector<core::Pose>& waypoints, core::NavigationFrame frame);
+
void pause();
void resume();
diff --git a/source/armarx/navigation/client/ice/NavigatorInterface.ice b/source/armarx/navigation/client/ice/NavigatorInterface.ice
index 678379d08e9189ed7270f988333e2f7f2bd1ab71..7f2a92d143f53727f90af626be2b6d6d55e35700 100644
--- a/source/armarx/navigation/client/ice/NavigatorInterface.ice
+++ b/source/armarx/navigation/client/ice/NavigatorInterface.ice
@@ -67,6 +67,10 @@ module armarx
void moveTo2(detail::Waypoints waypoints, string navigationFrame, string callerId);
+ void moveToLocation(string location, string callerId);
+
+ void updateMoveTo(Eigen::Matrix4fSeq waypoints, string navigationFrame, string callerId);
+
void
moveTowards(Eigen::Vector3f direction, string navigationFrame, string callerId);
diff --git a/source/armarx/navigation/client/services/EventSubscriptionInterface.h b/source/armarx/navigation/client/services/EventSubscriptionInterface.h
index 91fe872282030f36268ca55a82c67cef94d48334..e225f675c9dab1d8d7bda94dbf49a83eef7e3b70 100644
--- a/source/armarx/navigation/client/services/EventSubscriptionInterface.h
+++ b/source/armarx/navigation/client/services/EventSubscriptionInterface.h
@@ -1,14 +1,11 @@
#pragma once
-
-// STD/STL
#include <functional>
-// Navigation
#include <armarx/navigation/core/events.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::client
@@ -16,12 +13,15 @@ namespace armarx::navigation::client
using GlobalTrajectoryUpdatedCallback =
std::function<void(const global_planning::GlobalPlannerResult&)>;
- using LocalTrajectoryUpdatedCallback = std::function<void(const loc_plan::LocalPlannerResult&)>;
+ using LocalTrajectoryUpdatedCallback =
+ std::function<void(const local_planning::LocalPlannerResult&)>;
using TrajectoryControllerUpdatedCallback =
- std::function<void(const traj_ctrl::TrajectoryControllerResult&)>;
+ 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&)>;
@@ -52,9 +52,10 @@ 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.
- public:
virtual ~EventSubscriptionInterface() = default;
};
diff --git a/source/armarx/navigation/client/services/IceNavigator.cpp b/source/armarx/navigation/client/services/IceNavigator.cpp
index 965a59ee0e0b09932d03198e702bd31b241b423d..b07cf6a1f60f37d76f8bcc7a4f0e913cbc9bc9fd 100644
--- a/source/armarx/navigation/client/services/IceNavigator.cpp
+++ b/source/armarx/navigation/client/services/IceNavigator.cpp
@@ -2,6 +2,8 @@
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <armarx/navigation/core/types.h>
+
namespace armarx::navigation::client
{
@@ -65,9 +67,15 @@ namespace armarx::navigation::client
{
ARMARX_TRACE;
- return navigator->moveTo2(
+ navigator->moveTo2(
freeze(targets), core::NavigationFrameNames.to_name(navigationFrame), configId);
}
+
+ void IceNavigator::moveToLocation(const std::string& location)
+ {
+ ARMARX_TRACE;
+ navigator->moveToLocation(location, configId);
+ }
void
IceNavigator::moveTowards(const core::Direction& direction,
@@ -79,6 +87,16 @@ namespace armarx::navigation::client
direction, core::NavigationFrameNames.to_name(navigationFrame), configId);
}
+ void
+ IceNavigator::update(const std::vector<core::Pose>& waypoints,
+ core::NavigationFrame navigationFrame)
+ {
+ ARMARX_TRACE;
+
+ navigator->updateMoveTo(
+ convert(waypoints), core::NavigationFrameNames.to_name(navigationFrame), configId);
+ }
+
void
IceNavigator::pause()
{
diff --git a/source/armarx/navigation/client/services/IceNavigator.h b/source/armarx/navigation/client/services/IceNavigator.h
index 607500029bcbc29797cf56be15ce3280f060da37..f8cacfd7567bce8a260188e7761447ac36508e7d 100644
--- a/source/armarx/navigation/client/services/IceNavigator.h
+++ b/source/armarx/navigation/client/services/IceNavigator.h
@@ -7,6 +7,8 @@
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/client/ice_conversions.h>
#include <armarx/navigation/core/NavigatorInterface.h>
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/core/types.h>
namespace armarx::navigation::client
@@ -32,9 +34,14 @@ namespace armarx::navigation::client
void moveTo(const std::vector<client::WaypointTarget>& targets,
core::NavigationFrame navigationFrame) override;
+ void moveToLocation(const std::string& location) override;
+
void moveTowards(const core::Direction& direction,
core::NavigationFrame navigationFrame) override;
+ void update(const std::vector<core::Pose>& waypoints,
+ core::NavigationFrame navigationFrame) override;
+
void pause() override;
void resume() override;
diff --git a/source/armarx/navigation/client/services/MemorySubscriber.cpp b/source/armarx/navigation/client/services/MemorySubscriber.cpp
index ed515f00207d307adfa068eaf4a52f2dbe451589..8f08160106ed5c075bde977a5777dd696eabd87f 100644
--- a/source/armarx/navigation/client/services/MemorySubscriber.cpp
+++ b/source/armarx/navigation/client/services/MemorySubscriber.cpp
@@ -34,7 +34,6 @@ namespace armarx::navigation::client
<< ".";
// subscription api
- armem::MemoryID subscriptionID;
subscriptionID.memoryName = memoryName;
subscriptionID.coreSegmentName = "Events";
subscriptionID.providerSegmentName = callerId;
@@ -153,46 +152,15 @@ namespace armarx::navigation::client
MemorySubscriber::~MemorySubscriber()
{
- ARMARX_INFO << "Stopping event polling";
- task->stop();
- ARMARX_INFO << "done.";
- }
-
+ // ARMARX_INFO << "Removing event subscription";
- void
- MemorySubscriber::runPollMemoryEvents()
- {
ARMARX_TRACE;
+ // memoryNameSystem.unsubscribe(subscriptionID, this, &MemorySubscriber::onEntityUpdate);
- const armem::Time now = armem::Time::Now();
-
- armem::client::query::Builder qb;
- // clang-format off
- qb
- .coreSegments().withName("Events")
- .providerSegments().withName(callerId)
- .entities().all()
- .snapshots().timeRange(lastMemoryPoll, now);
- // clang-format on
-
- // ARMARX_DEBUG << "Polling memory events in interval "
- // << "[" << lastMemoryPoll << ", " << now << "]";
-
- lastMemoryPoll = now;
-
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
-
- // ARMARX_DEBUG << "Lookup result in reader: " << qResult;
-
- if (not qResult.success) /* c++20 [[unlikely]] */
- {
- ARMARX_WARNING << deactivateSpam(0.1F) << "Memory lookup failed.";
- return;
- }
-
- handleEvents(qResult.memory);
+ ARMARX_INFO << "done.";
}
+
void
MemorySubscriber::handleEvents(const armem::wm::Memory& memory)
{
diff --git a/source/armarx/navigation/client/services/MemorySubscriber.h b/source/armarx/navigation/client/services/MemorySubscriber.h
index a356cbbead76f0b235eee9dfb760de6d9e2c1227..1876bce97b50383928a8ac7d5e7643cabf33a82d 100644
--- a/source/armarx/navigation/client/services/MemorySubscriber.h
+++ b/source/armarx/navigation/client/services/MemorySubscriber.h
@@ -35,7 +35,6 @@ namespace armarx::navigation::client
protected:
- void runPollMemoryEvents();
void handleEvents(const armem::wm::Memory& memory);
@@ -51,7 +50,7 @@ namespace armarx::navigation::client
std::mutex eventHandlingMtx;
- armarx::PeriodicTask<MemorySubscriber>::pointer_type task;
+ armem::MemoryID subscriptionID;
};
} // namespace armarx::navigation::client
diff --git a/source/armarx/navigation/client/services/SimpleEventHandler.cpp b/source/armarx/navigation/client/services/SimpleEventHandler.cpp
index cda19d47b5804c313fba449d9f2dc78fd9ec7dcc..8406a84023585f2e5b738b3615b721c06d8e0279 100644
--- a/source/armarx/navigation/client/services/SimpleEventHandler.cpp
+++ b/source/armarx/navigation/client/services/SimpleEventHandler.cpp
@@ -55,11 +55,19 @@ armarx::navigation::client::SimpleEventHandler::SimpleEventHandler::onMovementSt
}
void
-armarx::navigation::client::SimpleEventHandler::onGlobalPlanningFailed(const GlobalPlanningFailedCallback& callback)
+armarx::navigation::client::SimpleEventHandler::onGlobalPlanningFailed(
+ const GlobalPlanningFailedCallback& callback)
{
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)
{
@@ -135,7 +143,7 @@ namespace armarx::navigation::client
}
void
- SimpleEventHandler::localTrajectoryUpdated(const loc_plan::LocalPlannerResult& event)
+ SimpleEventHandler::localTrajectoryUpdated(const local_planning::LocalPlannerResult& event)
{
for (const auto& callback : subscriptions.localTrajectoryUpdatedCallbacks)
{
@@ -143,20 +151,29 @@ namespace armarx::navigation::client
}
}
+ // void
+ // SimpleEventHandler::trajectoryControllerUpdated(
+ // const traj_ctrl::local::TrajectoryControllerResult& event)
+ // {
+ // for (const auto& callback : subscriptions.trajectoryControllerUpdatedCallbacks)
+ // {
+ // callback(event);
+ // }
+ // }
+
void
- SimpleEventHandler::trajectoryControllerUpdated(
- const traj_ctrl::TrajectoryControllerResult& event)
+ SimpleEventHandler::globalPlanningFailed(const core::GlobalPlanningFailedEvent& event)
{
- for (const auto& callback : subscriptions.trajectoryControllerUpdatedCallbacks)
+ for (const auto& callback : subscriptions.globalPlanningFailedCallbacks)
{
callback(event);
}
}
void
- SimpleEventHandler::globalPlanningFailed(const core::GlobalPlanningFailedEvent& event)
+ SimpleEventHandler::localPlanningFailed(const core::LocalPlanningFailedEvent& event)
{
- for (const auto& callback : subscriptions.globalPlanningFailedCallbacks)
+ for (const auto& callback : subscriptions.localPlanningFailedCallbacks)
{
callback(event);
}
diff --git a/source/armarx/navigation/client/services/SimpleEventHandler.h b/source/armarx/navigation/client/services/SimpleEventHandler.h
index 3f5fea9a70c79514f34cfb48377d7b3a50df68fa..a76a4a61a2094a581d24983c1cc961650ad9032d 100644
--- a/source/armarx/navigation/client/services/SimpleEventHandler.h
+++ b/source/armarx/navigation/client/services/SimpleEventHandler.h
@@ -14,8 +14,9 @@ namespace armarx::navigation::client
virtual public server::EventPublishingInterface
{
- // EventSubscriptionInterface
public:
+
+ // EventSubscriptionInterface
void onGoalReached(const OnGoalReachedCallback& callback) override;
void onWaypointReached(const OnWaypointReachedCallback& callback) override;
void
@@ -26,14 +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
- public:
void globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& event) override;
- void localTrajectoryUpdated(const loc_plan::LocalPlannerResult& event) override;
- void
- trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& 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;
@@ -50,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/common/controller_types.h b/source/armarx/navigation/common/controller_types.h
index 905ea6def15470f87aa55f44921629db0add69ff..444fca1e5f834aa7ab466eebae2cfa14b449466a 100644
--- a/source/armarx/navigation/common/controller_types.h
+++ b/source/armarx/navigation/common/controller_types.h
@@ -7,12 +7,16 @@ namespace armarx::navigation::common
enum class ControllerType
{
- PlatformTrajectory
+ PlatformGlobalTrajectory,
+ PlatformLocalTrajectory
};
- constexpr const char* PlatformTrajectoryControllerName= "PlatformTrajectory";
+ constexpr const char* PlatformGlobalTrajectoryControllerName = "PlatformGlobalTrajectory";
+ constexpr const char* PlatformLocalTrajectoryControllerName = "PlatformLocalTrajectory";
inline const simox::meta::EnumNames<ControllerType> ControllerTypeNames{
- {ControllerType::PlatformTrajectory, PlatformTrajectoryControllerName}};
+ {ControllerType::PlatformGlobalTrajectory, PlatformGlobalTrajectoryControllerName},
+ {ControllerType::PlatformLocalTrajectory, PlatformLocalTrajectoryControllerName}};
+
} // namespace armarx::navigation::common
diff --git a/source/armarx/navigation/components/CMakeLists.txt b/source/armarx/navigation/components/CMakeLists.txt
index 630b3cbdd20a8575529f1b20e164515c0d0f7e43..efa6ec80e5a10ff6f1a4667c6c4792b29ef71f7f 100644
--- a/source/armarx/navigation/components/CMakeLists.txt
+++ b/source/armarx/navigation/components/CMakeLists.txt
@@ -1,7 +1,23 @@
-add_subdirectory(GraphImportExport)
-add_subdirectory(NavigationMemory)
-add_subdirectory(Navigator)
+# Memory
+# ===================================
+
+add_subdirectory(graph_import_export)
+add_subdirectory(navigation_memory)
+
+# Navigator
+# ===================================
+add_subdirectory(navigator)
+
+# Costmap provider
+# ===================================
add_subdirectory(dynamic_distance_to_obstacle_costmap_provider)
+add_subdirectory(distance_to_obstacle_costmap_provider)
+
+# others
+# ===================================
+add_subdirectory(dynamic_scene_provider)
+
+add_subdirectory(human_simulator)
-add_subdirectory(navigation_skill_provider)
\ No newline at end of file
+add_subdirectory(navigation_skill_provider)
diff --git a/source/armarx/navigation/components/GraphImportExport/test/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/test/CMakeLists.txt
deleted file mode 100644
index c22cdacdcd31d554362a9e2024b9d5432aff7fe2..0000000000000000000000000000000000000000
--- a/source/armarx/navigation/components/GraphImportExport/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore GraphImportExport)
-
-armarx_add_test(GraphImportExportTest GraphImportExportTest.cpp "${LIBS}")
diff --git a/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/CMakeLists.txt b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a637fdcb41aecd9ca6352882e094fd48f1cd5d5d
--- /dev/null
+++ b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/CMakeLists.txt
@@ -0,0 +1,34 @@
+armarx_add_component(distance_to_obstacle_costmap_provider
+ ICE_FILES
+ ComponentInterface.ice
+ ICE_DEPENDENCIES
+ ArmarXCoreInterfaces
+ # RobotAPIInterfaces
+ # ARON_FILES
+ # aron/my_type.xml
+ SOURCES
+ Component.cpp
+ HEADERS
+ Component.h
+ DEPENDENCIES
+ # ArmarXCore
+ ArmarXCore
+ ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ # ArmarXGui
+ ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ # RobotAPI
+ armem_vision
+ armem_robot_state
+ armem_robot
+ ## RobotAPICore
+ ## RobotAPIInterfaces
+ RobotAPIComponentPlugins # For ArViz and other plugins.
+ armarx_navigation::memory
+ armarx_navigation::algorithms
+ armarx_navigation::util
+ # DEPENDENCIES_LEGACY
+ ## Add libraries that do not provide any targets but ${FOO_*} variables.
+ # FOO
+ # If you need a separate shared component library you can enable it with the following flag.
+ # SHARED_COMPONENT_LIBRARY
+)
diff --git a/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0221c606366d5a4992264907126e040303b2ae38
--- /dev/null
+++ b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.cpp
@@ -0,0 +1,268 @@
+/**
+ * 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::distance_to_obstacle_costmap_provider
+ * @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 <VirtualRobot/SceneObjectSet.h>
+
+#include "ArmarXCore/core/Component.h"
+#include "ArmarXCore/core/services/tasks/RunningTask.h"
+#include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/DateTime.h"
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/forward_declarations.h"
+
+#include <armarx/navigation/algorithms/CostmapBuilder.h>
+#include <armarx/navigation/util/util.h>
+
+
+namespace armarx::navigation::components::distance_to_obstacle_costmap_provider
+{
+ Component::Component()
+ {
+ addPlugin(virtualRobotReaderPlugin);
+ addPlugin(costmapWriterPlugin);
+ }
+
+
+ const std::string Component::defaultName = "distance_to_obstacle_costmap_provider";
+
+
+ armarx::PropertyDefinitionsPtr
+ Component::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def =
+ new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+
+ def->optional(properties.robotName, "p.robotName", "Robot name.");
+
+ 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();
+ }
+ */
+
+ runningTask = new RunningTask<Component>(this, &Component::run, "createAndStoreCostmap");
+ runningTask->start();
+ }
+
+
+ void
+ Component::run()
+ {
+ // FIXME: whenever the static scene changes, update the costmap
+
+ createAndStoreCostmap();
+ }
+
+ bool
+ Component::createAndStoreCostmap()
+ {
+ auto robot = virtualRobotReaderPlugin->get().getRobot(
+ properties.robotName,
+ armarx::DateTime::Invalid(),
+ VirtualRobot::RobotIO::RobotDescription::eCollisionModel);
+
+ const auto objectPoseClient = ObjectPoseClientPluginUser::getClient();
+
+ const objpose::ObjectPoseSeq objectPoses = objectPoseClient.fetchObjectPoses();
+
+ // remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
+ const auto objectPosesStatic =
+ armarx::navigation::util::filterObjects(objectPoses, {"KIT", "HOPE", "MDB", "YCB"});
+
+ const auto objects = armarx::navigation::util::asSceneObjects(objectPosesStatic);
+
+ ARMARX_CHECK_NOT_NULL(objects);
+ ARMARX_INFO << objects->getSize() << " objects in the scene";
+
+ ARMARX_INFO << "Creating costmap";
+ ARMARX_CHECK_NOT_NULL(robot);
+
+ // FIXME: move costmap creation out of this component
+ // FIXME create costmap writer enum: type of costmaps
+ algorithms::CostmapBuilder costmapBuilder(
+ robot,
+ objects,
+ algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
+ "Platform-navigation-colmodel");
+
+ const auto costmap = costmapBuilder.create();
+
+ ARMARX_INFO << "Storing costmap in the memory.";
+ return costmapWriterPlugin->get().store(
+ costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
+ }
+
+
+ void
+ Component::onDisconnectComponent()
+ {
+ runningTask->stop();
+ }
+
+
+ void
+ Component::onExitComponent()
+ {
+ }
+
+
+ std::string
+ Component::getDefaultName() const
+ {
+ return Component::defaultName;
+ }
+
+
+ std::string
+ Component::GetDefaultName()
+ {
+ return Component::defaultName;
+ }
+
+
+ /* (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::distance_to_obstacle_costmap_provider
diff --git a/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.h b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.h
new file mode 100644
index 0000000000000000000000000000000000000000..2f9a751a3a8f302b7d654be261d60d9b3f98d6ea
--- /dev/null
+++ b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.h
@@ -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/>.
+ *
+ * @package navigation::ArmarXObjects::distance_to_obstacle_costmap_provider
+ * @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 "ArmarXCore/core/services/tasks/RunningTask.h"
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+#include <armarx/navigation/memory/client/costmap/Writer.h>
+#include <armarx/navigation/components/distance_to_obstacle_costmap_provider/ComponentInterface.h>
+
+
+namespace armarx::navigation::components::distance_to_obstacle_costmap_provider
+{
+
+ class Component :
+ virtual public armarx::Component,
+ virtual public ObjectPoseClientPluginUser,
+ virtual public armarx::navigation::components::distance_to_obstacle_costmap_provider::
+ 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;
+ */
+
+ bool createAndStoreCostmap();
+
+ void run();
+
+ 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);
+ */
+
+
+ static const std::string defaultName;
+
+
+ // Private member variables go here.
+
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ std::string robotName = "Armar6";
+ };
+ 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;
+ */
+ std::optional<armem::robot::RobotDescription> robotDescription;
+ armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>*
+ virtualRobotReaderPlugin = nullptr;
+
+
+ armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Writer>*
+ costmapWriterPlugin = nullptr;
+
+
+ RunningTask<Component>::pointer_type runningTask;
+ };
+
+} // namespace armarx::navigation::components::distance_to_obstacle_costmap_provider
diff --git a/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/ComponentInterface.ice b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/ComponentInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..469a4290af0e9ee1b44b94b2b9d35781ba37c729
--- /dev/null
+++ b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/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::distance_to_obstacle_costmap_provider
+ * 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 distance_to_obstacle_costmap_provider
+{
+
+ interface ComponentInterface
+ {
+ // Define your interface here.
+ };
+
+};};};};
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 3ed827c9c3b60575d420ebef87a4ea9e8bcb6ed5..ab0d58102f6d3514940cabac0de4e8bea7d22f4f 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
@@ -37,11 +37,11 @@
#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h>
-#include "armarx/navigation/util/geometry.h"
+#include <armarx/navigation/util/geometry.h>
#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.
@@ -79,6 +79,16 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// def->optional(
// properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
def->optional(properties.updatePeriodMs, "p.updatePeriodMs", "");
+ def->optional(properties.staticCostmap.name, "p.staticCostmap.name", "");
+ 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.robot.name, "p.robot.name", "");
+
costmapReader.registerPropertyDefinitions(def);
costmapWriter.registerPropertyDefinitions(def);
@@ -159,13 +169,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_distance_to_obstacle_costmap_provider/Component.h b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
index 6094a09eb1b1b84849c7677c029837d43f8c8bba..1837d8ac9cf76718fef5978213a5786235a50a08 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
@@ -124,7 +124,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// std::string boxLayerName = "boxes";
struct
{
- std::string providerName = "navigator";
+ std::string providerName;
std::string name = "distance_to_obstacles";
} staticCostmap;
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.cpp b/source/armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..01e6728c6a2aa8868f1c7e96791ceffb389ea786
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.cpp
@@ -0,0 +1,62 @@
+#include "ArVizDrawer.h"
+
+#include <iomanip>
+#include <sstream>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+
+#include <RobotAPI/components/ArViz/Client/Client.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/Mesh.h>
+#include <RobotAPI/components/ArViz/Client/elements/PointCloud.h>
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+ ArVizDrawer::ArVizDrawer(armarx::viz::Client& arviz) :
+ arviz(arviz)
+ {
+ }
+
+ ArVizDrawer::~ArVizDrawer()
+ {
+ // make sure, the Arviz Ice calls are handled.
+ // {
+ // std::unique_lock g{layerMtx};
+ // g.lock();
+ // }
+ }
+
+ inline std::string
+ nameWithId(const std::string& name, const int id, const unsigned int idWidth = 6)
+ {
+ std::stringstream ss;
+ ss << name << "_" << std::setw(static_cast<int>(idWidth)) << std::setfill('0') << id;
+ return ss.str();
+ }
+
+ inline std::string nameWithIds(const std::string& name,
+ const int id,
+ const int subId,
+ const unsigned int idWidth = 6)
+ {
+ std::stringstream ss;
+ ss << name << "_" << std::setw(static_cast<int>(idWidth)) << std::setfill('0') << id << "_"
+ << std::setw(static_cast<int>(idWidth)) << std::setfill('0') << subId;
+ return ss.str();
+ }
+
+ void ArVizDrawer::visualizeFoo()
+ {
+ auto layer = arviz.layer("my_layer_name");
+
+ // TODO: populate layer, e.g.
+ // odomLayer.add(viz::Pose("odom").pose(odomPose.translation() * 1000, odomPose.linear()));
+
+ arviz.commit(layer);
+ }
+
+} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h b/source/armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h
new file mode 100644
index 0000000000000000000000000000000000000000..832fdeea3ad1b76f4a95e2f5e8c060d12e89ff0e
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h
@@ -0,0 +1,52 @@
+/*
+ * 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 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <Eigen/Geometry>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+
+namespace armarx::viz
+{
+ struct Client;
+} // namespace armarx::viz
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+ class ArVizDrawer
+ {
+ public:
+ ArVizDrawer(armarx::viz::Client& arviz);
+ virtual ~ArVizDrawer();
+
+ void visualizeFoo();
+
+ private:
+ viz::Client& arviz;
+ };
+
+
+} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..52e4efdccb9d044cc6262f8953838d00e53b3043
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -0,0 +1,43 @@
+armarx_add_component(dynamic_scene_provider
+ ICE_FILES
+ ComponentInterface.ice
+ ICE_DEPENDENCIES
+ ArmarXCoreInterfaces
+ # RobotAPIInterfaces
+ # ARON_FILES
+ # aron/my_type.xml
+ SOURCES
+ Component.cpp
+ ArVizDrawer.cpp
+ HEADERS
+ Component.h
+ ArVizDrawer.h
+ DEPENDENCIES
+ # ArmarXCore
+ ArmarXCore
+ ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ # ArmarXGui
+ ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ # 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
+ ## RobotAPICore
+ ## RobotAPIInterfaces
+ ## RobotAPIComponentPlugins # For ArViz and other plugins.
+ # DEPENDENCIES_PRIVATE
+ range-v3::range-v3
+ # DEPENDENCIES_LEGACY
+ ## Add libraries that do not provide any targets but ${FOO_*} variables.
+ # FOO
+ # If you need a separate shared component library you can enable it with the following flag.
+ # SHARED_COMPONENT_LIBRARY
+)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..77d1fd7f77f556efc19e553500ab14cd57d609c3
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -0,0 +1,530 @@
+/**
+ * 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::dynamic_scene_provider
+ * @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 <VirtualRobot/SceneObjectSet.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/time/Clock.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include <ArmarXCore/util/time.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+
+#include <VisionX/libraries/armem_human/client/HumanPoseReader.h>
+
+#include <armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h>
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/util/util.h>
+
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+ const std::string Component::defaultName = "dynamic_scene_provider";
+
+
+ Component::Component()
+ {
+ addPlugin(humanPoseReaderPlugin);
+ addPlugin(laserScannerFeaturesReaderPlugin);
+ addPlugin(virtualRobotReaderPlugin);
+ addPlugin(costmapReaderPlugin);
+ addPlugin(occupancyGridReaderPlugin);
+ addPlugin(humanWriterPlugin);
+ }
+
+ 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", "Update rate of the running task.");
+
+ def->optional(properties.laserScannerFeatures.providerName,
+ "p.laserScannerFeatures.providerName",
+ "");
+ def->optional(properties.laserScannerFeatures.name, "p.laserScannerFeatures.name", "");
+
+ def->optional(properties.robot.name, "p.robot.name", "");
+
+ def->optional(properties.occupancyGrid.providerName, "p.occupancyGrid.providerName", "");
+ def->optional(properties.occupancyGrid.name, "p.occupancyGrid.name", "");
+ def->optional(
+ properties.occupancyGrid.freespaceThreshold, "p.occupancyGrid.freespaceThreshold", "");
+ def->optional(
+ properties.occupancyGrid.occupiedThreshold, "p.occupancyGrid.occupiedThreshold", "");
+
+ def->optional(properties.humanPoseProvider, "p.humanPoseProvider", "");
+
+ 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()
+ {
+ arvizDrawer.emplace(ArVizDrawer(getArvizClient()));
+
+ // 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();
+ }
+ */
+
+ 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();
+
+ 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()
+ {
+ TIMING_START(FULL_DYNAMIC_SCENE_PROVIDER);
+
+ // obtain data from perception
+
+ const DateTime timestamp = Clock::Now();
+
+
+ //
+ // Robot
+ //
+
+ TIMING_START(READ_ROBOT_FROM_MEMORY);
+
+ ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp));
+ const core::Pose global_T_robot(robot->getGlobalPose());
+
+ ARMARX_VERBOSE << "Robot position: " << global_T_robot.translation().head<2>();
+
+ TIMING_END_COMMENT_STREAM(
+ READ_ROBOT_FROM_MEMORY, "Timer: reading robot from memory", ARMARX_VERBOSE);
+
+
+ //
+ // Human
+ //
+
+ TIMING_START(READ_HUMANS_FROM_MEMORY);
+
+ ARMARX_VERBOSE << "Querying humans";
+
+ const armem::human::client::Reader::Query humanPoseQuery{
+ .providerName = properties.humanPoseProvider,
+ .timestamp = timestamp,
+ .maxAge = Duration::MilliSeconds(500)};
+
+ const armem::human::client::Reader::Result humanPoseResult =
+ humanPoseReaderPlugin->get().query(humanPoseQuery);
+ ARMARX_CHECK_NOT_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Error)
+ << humanPoseResult.errorMessage;
+
+ ARMARX_VERBOSE << humanPoseResult.humanPoses.size() << " humans in the scene.";
+
+ TIMING_END_COMMENT_STREAM(
+ READ_HUMANS_FROM_MEMORY, "Timer: reading humans from memory", ARMARX_VERBOSE);
+
+
+ //
+ // Laser scanner features
+ //
+
+ /*
+ TIMING_START(READ_LASERSCANNER_FROM_MEMORY);
+
+ ARMARX_INFO << "Querying laser scanner features";
+ const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery{
+ .providerName = properties.laserScannerFeatures.providerName,
+ .name = properties.laserScannerFeatures.name,
+ .timestamp = timestamp};
+
+ const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
+ laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
+
+ ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
+ armem::vision::laser_scanner_features::client::Reader::Result::Success);
+
+ ARMARX_INFO << laserFeaturesResult.features.size() << " clusters/features";
+
+ TIMING_END_COMMENT_STREAM(
+ READ_LASERSCANNER_FROM_MEMORY, "Timer: reading laserscanner from memory", ARMARX_VERBOSE);
+ */
+
+
+ /* we don't need this at the moment
+
+ //
+ // Objects in the scene (both static and dynamic)
+ //
+ ARMARX_INFO << "Querying object poses";
+
+ const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
+
+ // remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
+ const auto objectPosesStatic =
+ armarx::navigation::util::filterObjects(objectPoses, {"KIT", "HOPE", "MDB", "YCB"});
+
+ const auto objects = armarx::navigation::util::asSceneObjects(objectPosesStatic);
+ ARMARX_INFO << objects->getSize() << " objects in the scene";
+
+ // ARMARX_INFO << "Creating costmap";
+ // ARMARX_CHECK_NOT_NULL(scene.robot);
+
+ // algorithms::CostmapBuilder costmapBuilder(
+ // scene.robot,
+ // objects,
+ // algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
+ // "Platform-navigation-colmodel");
+
+ // // const auto costmap = costmapBuilder.create();
+
+ //
+ // Costmaps
+ //
+
+ ARMARX_INFO << "Querying costmap";
+
+ 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);
+
+ ARMARX_CHECK_EQUAL(costmapResult.status, memory::client::costmap::Reader::Result::Success);
+
+ ARMARX_TRACE;
+ ARMARX_CHECK(costmapResult.costmap.has_value());
+ const auto grid = costmapResult.costmap->getGrid();
+
+
+ //
+ // Occupancy grid: from SLAM component
+ //
+
+ const armem::vision::occupancy_grid::client::Reader::Result result =
+ occupancyGridReaderPlugin->get().query(
+ armem::vision::occupancy_grid::client::Reader::Query{
+ .providerName = properties.occupancyGrid.providerName,
+ .timestamp = armem::Time::Now()});
+
+ if (result and result.occupancyGrid.has_value())
+ {
+ ARMARX_INFO << "Occupancy grid available!";
+
+ const auto occupancyGridSceneElements = util::asSceneObjects(
+ result.occupancyGrid.value(),
+ OccupancyGridHelper::Params{
+ .freespaceThreshold = properties.occupancyGrid.freespaceThreshold,
+ .occupiedThreshold = properties.occupancyGrid.occupiedThreshold});
+ ARMARX_INFO << occupancyGridSceneElements->getSize()
+ << " scene elements from occupancy grid";
+
+ auto occupancyGridObstacles =
+ std::make_shared<VirtualRobot::SceneObjectSet>("OccupancyGridObstacles");
+ occupancyGridObstacles->addSceneObjects(occupancyGridSceneElements);
+
+ // draw
+ // auto layer = arviz.layer("occupancy_grid");
+
+ // for (const auto& sceneObject : occupancyGridSceneElements->getSceneObjects())
+ // {
+ // const Eigen::Isometry3f world_T_obj(sceneObject->getGlobalPose());
+ // ARMARX_INFO << world_T_obj.translation();
+ // ARMARX_INFO << layer.size();
+ // layer.add(viz::Box("box_" + std::to_string(layer.size()))
+ // .pose(world_T_obj)
+ // .size(result.occupancyGrid->resolution)
+ // .color(viz::Color::orange()));
+ // }
+
+ // ARMARX_INFO << "Creating costmap";
+
+ // algorithms::CostmapBuilder costmapBuilder(
+ // getRobot(),
+ // scene.objects,
+ // algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
+ // "Platform-navigation-colmodel");
+
+ // const auto costmap = costmapBuilder.create();
+
+ // ARMARX_INFO << "Done";
+
+ // ARMARX_TRACE;
+ // ARMARX_INFO << "Saving costmap.";
+ // algorithms::save(costmap, "/tmp/navigation-costmap");
+
+ // arviz.commit({layer});
+ }
+
+ // here ends: data fetching
+
+ */
+
+
+ // process data from perception and write information back to memory
+
+ //
+ // Human tracking
+ //
+
+ TIMING_START(RUNNING_HUMAN_TRACKER_WITH_CAMERA);
+
+ ARMARX_VERBOSE << "Running human tracker with camera measurements";
+
+ humanTracker.update(human::HumanTracker::CameraMeasurement{
+ .detectionTime = timestamp, .humanPoses = humanPoseResult.humanPoses});
+
+ TIMING_END_COMMENT_STREAM(RUNNING_HUMAN_TRACKER_WITH_CAMERA,
+ "Timer: running human tracker with camera",
+ ARMARX_VERBOSE);
+
+
+ /*
+ ARMARX_INFO << "Running human tracker with lasersensor measurements";
+
+ TIMING_START(RUNNING_HUMAN_TRACKER_WITH_LASERSCANNER);
+
+ //TODO why is result a vector of LSFs and not a vector of LSF?
+ std::vector<armem::vision::LaserScannerFeature> flattened;
+ for (auto const& fs : laserFeaturesResult.features)
+ {
+ flattened.insert(flattened.end(), fs.features.begin(), fs.features.end());
+ }
+ std::vector<armem::vision::LaserScannerFeature> clusters =
+ humanTracker.update(human::HumanTracker::LaserMeasurement{.detectionTime = timestamp,
+ .clusters = flattened});
+
+ TIMING_END_COMMENT_STREAM(RUNNING_HUMAN_TRACKER_WITH_LASERSCANNER,
+ "Timer: running human tracker with laserscanner",
+ ARMARX_VERBOSE);
+ */
+
+ ARMARX_VERBOSE << "Human tracking done";
+
+
+ //
+ // Write dynamic scene back to memory
+ //
+
+ TIMING_START(WRITE_BACK_HUMANS);
+
+ //TODO use clusters for obstacle creation
+
+ std::vector<human::Human> humans = humanTracker.getTrackedHumans();
+
+
+ if(not humans.empty())
+ {
+ ARMARX_INFO << "Detected " << humans.size() << " humans";
+ humanWriterPlugin->get().store(humans, getName(), timestamp);
+ }
+
+
+ TIMING_END_COMMENT_STREAM(
+ WRITE_BACK_HUMANS, "Timer: write humans to memory", ARMARX_VERBOSE);
+
+
+ TIMING_END_COMMENT_STREAM(
+ FULL_DYNAMIC_SCENE_PROVIDER, "Timer: complete dynamic scene provider", ARMARX_VERBOSE);
+ }
+
+
+ /* (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::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
new file mode 100644
index 0000000000000000000000000000000000000000..11e0d65404a2c24d6179fe6327ee3e7b32fcc5c6
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -0,0 +1,211 @@
+/**
+ * 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::dynamic_scene_provider
+ * @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 <VirtualRobot/VirtualRobot.h>
+
+#include "ArmarXCore/core/services/tasks/TaskUtil.h"
+#include <ArmarXCore/core/Component.h>
+
+#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
+
+#include "VisionX/libraries/armem_human/client/HumanPoseReader.h"
+
+// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include "RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h"
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include <armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h>
+#include <armarx/navigation/human/HumanTracker.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/memory/client/human/Writer.h>
+#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
+
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+ class Component :
+ virtual public armarx::Component,
+ virtual public armarx::navigation::components::dynamic_scene_provider::ComponentInterface,
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::armem::client::plugins::PluginUser,
+ virtual public ObjectPoseClientPluginUser
+ {
+ 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();
+
+ VirtualRobot::RobotPtr robot = nullptr;
+
+
+ private:
+ static const std::string defaultName;
+
+
+ // Private member variables go here.
+
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ int taskPeriodMs = 100;
+
+ struct
+ {
+ std::string providerName = "LaserScannerFeatureExtraction";
+ std::string name = ""; // all
+ } laserScannerFeatures;
+
+
+ struct
+ {
+ std::string name = "Armar6";
+ } robot;
+
+ struct
+ {
+ std::string providerName = "CartographerMappingAndLocalization";
+ std::string name = ""; // all
+
+ float freespaceThreshold = 0.45F;
+ float occupiedThreshold = 0.55;
+ } occupancyGrid;
+
+ std::string humanPoseProvider = "AzureKinectPointCloudProvider";
+ };
+ 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;
+ */
+
+ std::optional<ArVizDrawer> arvizDrawer;
+
+
+ PeriodicTask<Component>::pointer_type task;
+
+ template <typename T>
+ using ReaderWriterPlugin = armem::client::plugins::ReaderWriterPlugin<T>;
+
+ ReaderWriterPlugin<armem::human::client::Reader>* humanPoseReaderPlugin = nullptr;
+
+ ReaderWriterPlugin<armem::vision::laser_scanner_features::client::Reader>*
+ laserScannerFeaturesReaderPlugin = nullptr;
+
+ ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin =
+ nullptr;
+
+ ReaderWriterPlugin<memory::client::costmap::Reader>* costmapReaderPlugin = nullptr;
+
+ ReaderWriterPlugin<armem::vision::occupancy_grid::client::Reader>*
+ occupancyGridReaderPlugin = nullptr;
+
+ ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
+
+
+ human::HumanTracker humanTracker;
+ };
+
+} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/ComponentInterface.ice b/source/armarx/navigation/components/dynamic_scene_provider/ComponentInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..a66c4f5615124e5682ba0fe06f5deb99f8e3363d
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/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::dynamic_scene_provider
+ * 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 dynamic_scene_provider
+{
+
+ interface ComponentInterface
+ {
+ // Define your interface here.
+ };
+
+};};};};
diff --git a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt b/source/armarx/navigation/components/graph_import_export/CMakeLists.txt
similarity index 88%
rename from source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
rename to source/armarx/navigation/components/graph_import_export/CMakeLists.txt
index 123769e24867dff0c0a6026262560197e8e514ae..44a43ae8765104b4c7a8a3eaab1ccd1356aeb60d 100644
--- a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
+++ b/source/armarx/navigation/components/graph_import_export/CMakeLists.txt
@@ -1,4 +1,4 @@
-armarx_add_component(GraphImportExport
+armarx_add_component(graph_import_export
DEPENDENCIES
# ArmarXCore
ArmarXCore
@@ -24,8 +24,8 @@ armarx_add_component(GraphImportExport
## GraphImportExportInterfaces # If you defined a component ice interface above.
SOURCES
- GraphImportExport.cpp
+ Component.cpp
HEADERS
- GraphImportExport.h
+ Component.h
)
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/graph_import_export/Component.cpp
similarity index 90%
rename from source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
rename to source/armarx/navigation/components/graph_import_export/Component.cpp
index c1976d582786e72afa4f3c3872b428893cfe9ab8..b4e8cbba10866858f565254a74c927891652215e 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
+++ b/source/armarx/navigation/components/graph_import_export/Component.cpp
@@ -20,7 +20,7 @@
* GNU General Public License
*/
-#include "GraphImportExport.h"
+#include "Component.h"
#include <iomanip>
@@ -42,11 +42,10 @@
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/location/constants.h>
-
-namespace armarx::navigation
+namespace armarx::navigation::components::graph_import_export
{
- GraphImportExport::GraphImportExport()
+ Component::Component()
{
properties.locationCoreSegmentID = location::coreSegmentID;
properties.graphCoreSegmentID = graph::coreSegmentID;
@@ -54,7 +53,7 @@ namespace armarx::navigation
armarx::PropertyDefinitionsPtr
- GraphImportExport::createPropertyDefinitions()
+ Component::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr def =
new ComponentPropertyDefinitions(getConfigIdentifier());
@@ -67,7 +66,7 @@ namespace armarx::navigation
void
- GraphImportExport::onInitComponent()
+ Component::onInitComponent()
{
// Topics and properties defined above are automagically registered.
@@ -77,7 +76,7 @@ namespace armarx::navigation
void
- GraphImportExport::onConnectComponent()
+ Component::onConnectComponent()
{
// Get proxies.
if (proxies.priorKnowledge->hasGraphSegment())
@@ -95,26 +94,32 @@ namespace armarx::navigation
void
- GraphImportExport::onDisconnectComponent()
+ Component::onDisconnectComponent()
{
}
void
- GraphImportExport::onExitComponent()
+ Component::onExitComponent()
{
}
std::string
- GraphImportExport::getDefaultName() const
+ Component::getDefaultName() const
+ {
+ return GetDefaultName();
+ }
+
+ std::string
+ Component::GetDefaultName()
{
- return "GraphImportExport";
+ return "graph_import_export";
}
void
- GraphImportExport::createRemoteGuiTab(const std::vector<std::string>& sceneNames)
+ Component::createRemoteGuiTab(const std::vector<std::string>& sceneNames)
{
using namespace armarx::RemoteGui::Client;
@@ -169,7 +174,7 @@ namespace armarx::navigation
void
- GraphImportExport::RemoteGui_update()
+ Component::RemoteGui_update()
{
if (tab.sceneRefreshButton.wasClicked())
{
@@ -205,7 +210,7 @@ namespace armarx::navigation
void
- GraphImportExport::refreshScenes()
+ Component::refreshScenes()
{
const ::Ice::StringSeq sceneNames = proxies.graphSegment->getScenes();
createRemoteGuiTab(sceneNames);
@@ -213,7 +218,7 @@ namespace armarx::navigation
void
- GraphImportExport::locationsMemoryxToArmem(const std::string& sceneName)
+ Component::locationsMemoryxToArmem(const std::string& sceneName)
{
const armem::Time time = armem::Time::Now();
armem::Commit commit;
@@ -276,7 +281,7 @@ namespace armarx::navigation
void
- GraphImportExport::locationsArmemToMemoryx(const std::string& sceneName)
+ Component::locationsArmemToMemoryx(const std::string& sceneName)
{
ARMARX_IMPORTANT << "locationsArmemToMemoryx() is WIP!";
(void)sceneName;
@@ -284,7 +289,7 @@ namespace armarx::navigation
void
- GraphImportExport::graphMemoryxToArmem(const std::string& sceneName)
+ Component::graphMemoryxToArmem(const std::string& sceneName)
{
memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName);
navigation::core::Graph graph = toArmemGraph(graphNodes);
@@ -331,7 +336,7 @@ namespace armarx::navigation
void
- GraphImportExport::graphArmemToMemoryx(const std::string& sceneName)
+ Component::graphArmemToMemoryx(const std::string& sceneName)
{
ARMARX_IMPORTANT << "graphArmemToMemoryx() is WIP!";
(void)sceneName;
@@ -339,8 +344,8 @@ namespace armarx::navigation
void
- GraphImportExport::clearArMemProviderSegment(armem::client::Writer& writer,
- const armem::MemoryID& providerSegmentID)
+ Component::clearArMemProviderSegment(armem::client::Writer& writer,
+ const armem::MemoryID& providerSegmentID)
{
const bool clearWhenExists = true;
auto result = writer.addSegment(providerSegmentID, clearWhenExists);
@@ -356,7 +361,7 @@ namespace armarx::navigation
armem::MemoryID
- GraphImportExport::getLocationProviderSegmentID()
+ Component::getLocationProviderSegmentID()
{
return properties.locationCoreSegmentID.withProviderSegmentName(
tab.providerSegmentLine.getValue());
@@ -364,7 +369,7 @@ namespace armarx::navigation
armem::MemoryID
- GraphImportExport::getGraphProviderSegmentID()
+ Component::getGraphProviderSegmentID()
{
return properties.graphCoreSegmentID.withProviderSegmentName(
tab.providerSegmentLine.getValue());
@@ -372,7 +377,7 @@ namespace armarx::navigation
navigation::core::Graph
- GraphImportExport::toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes)
+ Component::toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes)
{
navigation::core::Graph graph;
std::map<std::string, navigation::core::Graph::Vertex> vertexMap;
@@ -439,6 +444,7 @@ namespace armarx::navigation
}
- ARMARX_DECOUPLED_REGISTER_COMPONENT(GraphImportExport);
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
+
-} // namespace armarx::navigation
+} // namespace armarx::navigation::components::graph_import_export
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h b/source/armarx/navigation/components/graph_import_export/Component.h
similarity index 96%
rename from source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
rename to source/armarx/navigation/components/graph_import_export/Component.h
index c877740cb5ae4e8b4f432281386d400fa86031e4..45e704adf0ede2025e76ebccc8938c488001b2b1 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
+++ b/source/armarx/navigation/components/graph_import_export/Component.h
@@ -38,7 +38,7 @@
#include <armarx/navigation/core/forward_declarations.h>
-namespace armarx::navigation
+namespace armarx::navigation::components::graph_import_export
{
/**
@@ -52,7 +52,7 @@ namespace armarx::navigation
*
* Detailed description of class GraphImportExport.
*/
- class GraphImportExport :
+ class Component :
virtual public armarx::Component,
virtual public armarx::DebugObserverComponentPluginUser,
virtual public armarx::LightweightRemoteGuiComponentPluginUser,
@@ -60,11 +60,12 @@ namespace armarx::navigation
virtual public armarx::armem::ClientPluginUser
{
public:
- GraphImportExport();
+ Component();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+ static std::string GetDefaultName();
protected:
@@ -154,4 +155,4 @@ namespace armarx::navigation
};
RemoteGuiTab tab;
};
-} // namespace armarx::navigation
+} // namespace armarx::navigation::components::graph_import_export
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..c5cb14c6099a66e52447e5654af7aadbf138dc7d
--- /dev/null
+++ b/source/armarx/navigation/components/human_simulator/CMakeLists.txt
@@ -0,0 +1,29 @@
+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
+
+ # HRVO::HRVO
+ RVO::RVO
+)
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..ce8b62064e5f66b63f69d23e1ad48f0da7b491bf
--- /dev/null
+++ b/source/armarx/navigation/components/human_simulator/Component.cpp
@@ -0,0 +1,493 @@
+/**
+ * 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 <cstddef>
+#include <cstdlib>
+
+#include <SimoxUtility/color/Color.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/Random.h>
+#include <VirtualRobot/SceneObjectSet.h>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include "RobotAPI/components/ArViz/Client/Elements.h"
+#include "RobotAPI/components/ArViz/Client/elements/Path.h"
+#include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.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 <armarx/navigation/util/util.h>
+#include <RVOSimulator.h>
+#include <Vector2.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", "");
+
+ def->optional(properties.objectPoseProviderName, "p.objectPoseProviderName", "");
+
+ 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::setPreferredVelocities(RVO::RVOSimulator* simulator,
+ const std::vector<RVO::Vector2>& goals)
+ {
+ /* Set the preferred velocity to be a vector of unit magnitude (speed) in the
+ * direction of the goal. */
+ // #ifdef _OPENMP
+ // #pragma omp parallel for
+ // #endif /* _OPENMP */
+
+ for (int i = 0; i < static_cast<int>(properties.nHumans); ++i)
+ {
+ // RVO::Vector2 goalVector = goals[i] - simulator->getAgentPosition(i);
+
+ // if (RVO::absSq(goalVector) > 1.0F)
+ // {
+ // goalVector = RVO::normalize(goalVector);
+ // }
+
+ const auto agentPosition = simulator->getAgentPosition(i);
+ auto& simulatedHuman = simulatedHumans.at(i);
+
+ core::Pose2D pose = core::Pose2D::Identity();
+ pose.translation().x() = agentPosition.x() * 1000; // [m] -> [mm]
+ pose.translation().y() = agentPosition.y() * 1000; // [m] -> [mm]
+
+ simulatedHuman.reinitialize(pose);
+
+ const std::vector<core::Position> trajectoryPoints =
+ simulatedHuman.trajectory().positions();
+
+ const std::size_t lookAheadGoalIdx = std::min(trajectoryPoints.size() - 1, 5ul);
+ const auto& intermediateGoal = trajectoryPoints.at(lookAheadGoalIdx);
+
+ const Eigen::Vector2f rvoGoalHelper = intermediateGoal.head<2>() / 1000; // [mm] -> [m]
+
+ const RVO::Vector2 rvoGoal(rvoGoalHelper.x(), rvoGoalHelper.y());
+
+ RVO::Vector2 goalVector = rvoGoal - agentPosition;
+
+ if (RVO::absSq(goalVector) > 1.0F)
+ {
+ goalVector = RVO::normalize(goalVector);
+ }
+
+ simulator->setAgentPrefVelocity(i, goalVector);
+ }
+ }
+
+
+ void
+ Component::onConnectComponent()
+ {
+ //
+ // 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 simulation environment
+ {
+ ARMARX_INFO << "Creating sim";
+
+ simulator.setTimeStep(static_cast<float>(properties.taskPeriodMs) / 1000);
+ simulator.setAgentDefaults(15.0F, 10U, 1.5F, .1F, .4F, .5F);
+
+ const objpose::ObjectPoseSeq obstacles =
+ ObjectPoseClientPluginUser::getObjectPosesByProvider(
+ properties.objectPoseProviderName);
+
+ const auto objects = armarx::navigation::util::asSceneObjects(obstacles);
+
+
+ ARMARX_VERBOSE << obstacles.size() << " obstacles";
+
+ {
+ auto layer = arviz.layer("obstacles");
+
+
+ for (const auto& obst : objects->getCollisionModels())
+ {
+
+ ARMARX_INFO << "adding obstacle ...";
+ const VirtualRobot::BoundingBox bbox = obst->getGlobalBoundingBox();
+
+ Eigen::Vector2f bbMin = bbox.getMin().head<2>();
+ Eigen::Vector2f bbMax = bbox.getMax().head<2>();
+
+ // add some margin to the obstacles
+ const float margin = 100; // [mm]
+
+ bbMin.x() -= margin;
+ bbMin.y() -= margin;
+ bbMax.x() += margin;
+ bbMax.y() += margin;
+
+ ARMARX_VERBOSE << "Bounding box: (" << bbMin.transpose() << "), ("
+ << bbMax.transpose() << ")";
+
+ {
+ constexpr float zHeight = 3; // [mm]
+
+ viz::Polygon polygon(std::to_string(layer.size()));
+ polygon.addPoint(Eigen::Vector3f{bbMin.x(), bbMin.y(), zHeight});
+ polygon.addPoint(Eigen::Vector3f{bbMax.x(), bbMin.y(), zHeight});
+ polygon.addPoint(Eigen::Vector3f{bbMax.x(), bbMax.y(), zHeight});
+ polygon.addPoint(Eigen::Vector3f{bbMin.x(), bbMax.y(), zHeight});
+
+ polygon.color(simox::Color::black());
+ polygon.color(simox::Color::black());
+
+ layer.add(polygon);
+ }
+
+
+ const Eigen::Vector2d min = bbMin.cast<double>() / 1000; // [mm] -> [m]
+ const Eigen::Vector2d max = bbMax.cast<double>() / 1000; // [mm] -> [m]
+
+ std::vector<RVO::Vector2> obstacle;
+ obstacle.emplace_back(min.x(), min.y());
+ obstacle.emplace_back(max.x(), min.y());
+ obstacle.emplace_back(max.x(), max.y());
+ obstacle.emplace_back(min.x(), max.y());
+
+ simulator.addObstacle(obstacle);
+ }
+
+ arviz.commit(layer);
+ }
+
+ simulator.processObstacles();
+ }
+
+ /// create humans in simulation
+ {
+ simulatedHumans.clear();
+
+ for (std::size_t i = 0U; i < properties.nHumans; ++i)
+ {
+ armarx::navigation::human::simulation::SimulatedHuman& human =
+ simulatedHumans.emplace_back(costmap);
+
+ const RVO::Vector2 position =
+ RVO::Vector2(human.human().pose.translation().x() / 1000,
+ human.human().pose.translation().y() / 1000);
+ auto goal = human.trajectory().positions().back() / 1000; // [mm] -> [m]
+
+ ARMARX_VERBOSE << "Adding agent";
+ // simulator.addAgent(position, simulator.addGoal(hrvo::Vector2(goal.x(), goal.y())));
+ simulator.addAgent(
+ position); // , simulator.addGoal(hrvo::Vector2(goal.x(), goal.y())));
+
+ goals.emplace_back(goal.x(), goal.y());
+ }
+ }
+
+ /// load robot and add it to the simulator
+ {
+ robot = virtualRobotReaderPlugin->get().getRobot(properties.robotName);
+ ARMARX_CHECK(
+ virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now()));
+
+ const auto robotPosH = robot->getGlobalPosition();
+ const RVO::Vector2 robotPos(robotPosH.x() / 1000, robotPosH.y() / 1000);
+
+ robotAgentId = simulator.addAgent(robotPos);
+ simulator.setAgentMaxSpeed(robotAgentId,
+ 0); // we do not allow the robot to be perturbed
+ simulator.setAgentPrefVelocity(robotAgentId, RVO::Vector2(0, 0));
+ simulator.setAgentRadius(robotAgentId, 0.6);
+ }
+
+ /// start task
+ 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()
+ {
+ const DateTime timestamp = Clock::Now();
+
+ setPreferredVelocities(&simulator, goals);
+
+ // TODO(fabian.reister): check how much the agents interact => this can be an evaluation criterion
+
+ // update robot
+ {
+ ARMARX_CHECK(
+ virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now()));
+
+ const auto robotPosH = robot->getGlobalPosition();
+ const RVO::Vector2 robotPos(robotPosH.x() / 1000, robotPosH.y() / 1000);
+
+ simulator.setAgentPosition(robotAgentId, robotPos);
+ }
+
+ simulator.doStep();
+
+ // visualize paths
+ {
+ auto layer = arviz.layer("trajectories");
+ for (const auto& simulatedHuman : simulatedHumans)
+ {
+ viz::Path path(std::to_string(layer.size()));
+
+ for (const auto& pt : simulatedHuman.trajectory().points())
+ {
+ path.addPoint(pt.waypoint.pose.translation());
+ }
+
+ // unique and distinct color for this human
+ path.colorGlasbeyLUT(layer.size());
+
+ layer.add(path);
+ }
+
+ arviz.commit(layer);
+ }
+
+
+ human::Humans humans;
+ for (std::size_t i = 0; i < properties.nHumans; i++)
+ {
+ const auto pos = simulator.getAgentPosition(i);
+
+ human::Human human;
+ human.pose.setIdentity();
+ human.pose.translation().x() = pos.x() * 1000; // [m] -> [mm]
+ human.pose.translation().y() = pos.y() * 1000; // [m] -> [mm]
+
+ humans.push_back(human);
+ }
+
+ 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..d0627617447b4cbaa8dcc541631bd8b19257821f
--- /dev/null
+++ b/source/armarx/navigation/components/human_simulator/Component.h
@@ -0,0 +1,191 @@
+/**
+ * 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 <string>
+#include <VirtualRobot/VirtualRobot.h>
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include <ArmarXCore/core/Component.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.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/memory/client/costmap/Reader.h>
+#include <armarx/navigation/memory/client/human/Writer.h>
+#include <armarx/navigation/simulation/SimulatedHuman.h>
+// #include <Simulator.h>
+#include "RVO.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::ObjectPoseClientPluginUser,
+ // , 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();
+
+ void setPreferredVelocities(RVO::RVOSimulator* simulator,
+ const std::vector<RVO::Vector2>& goals);
+
+
+ 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;
+
+ std::string robotName = "Armar6";
+
+ std::string objectPoseProviderName = "R003_grasping_challenge";
+ };
+ 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>;
+
+ VirtualRobot::RobotPtr robot;
+
+ std::size_t robotAgentId;
+
+ 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;
+
+ // hrvo::Simulator simulator;
+ RVO::RVOSimulator simulator;
+
+ std::vector<RVO::Vector2> goals;
+ };
+
+} // 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/NavigationMemory/CMakeLists.txt b/source/armarx/navigation/components/navigation_memory/CMakeLists.txt
similarity index 85%
rename from source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
rename to source/armarx/navigation/components/navigation_memory/CMakeLists.txt
index 8d83974fdcc3068be6b78069b21d0b0f2a4c47b2..b4305d4ca486a37ac912d7846aa7b8fdfa05a890 100644
--- a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
+++ b/source/armarx/navigation/components/navigation_memory/CMakeLists.txt
@@ -22,13 +22,16 @@ armarx_add_component(navigation_memory
## NavigationMemoryInterfaces # If you defined a component ice interface above.
armarx_navigation::graph
armarx_navigation::location
+ armarx_navigation::algorithms
+ armarx_navigation::teb_human
armarx_navigation::rooms
+ armarx_navigation::memory
SOURCES
- NavigationMemory.cpp
+ Component.cpp
Visu.cpp
HEADERS
- NavigationMemory.h
+ Component.h
Visu.h
)
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/navigation_memory/Component.cpp
similarity index 89%
rename from source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
rename to source/armarx/navigation/components/navigation_memory/Component.cpp
index 650f119bc2fc88774af658e046d5545bacf1ebbe..22f674ca75fbe8e24770ad6cfb47d12efa814cee 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Component.cpp
@@ -20,19 +20,16 @@
* GNU General Public License
*/
-#include "NavigationMemory.h"
+#include "Component.h"
-// STD / STL
-#include <cstddef>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <iterator>
-#include <SimoxUtility/json/json.hpp>
-
-#include "ArmarXCore/core/PackagePath.h"
-#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/Component.h"
+#include "ArmarXCore/core/time/Metronome.h"
+#include "ArmarXCore/core/time/forward_declarations.h"
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
@@ -47,7 +44,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include "Visu.h"
-#include "armarx/navigation/rooms/constants.h"
+#include <armarx/navigation/rooms/constants.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/core/Graph.h>
@@ -55,19 +52,19 @@
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
#include <armarx/navigation/core/aron/Twist.aron.generated.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/location/constants.h>
#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/rooms/aron/Room.aron.generated.h>
-namespace armarx::navigation
+namespace armarx::navigation::components::navigation_memory
{
- ARMARX_DECOUPLED_REGISTER_COMPONENT(NavigationMemory);
-
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
armarx::PropertyDefinitionsPtr
- NavigationMemory::createPropertyDefinitions()
+ Component::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr def =
new ComponentPropertyDefinitions(getConfigIdentifier());
@@ -91,11 +88,29 @@ namespace armarx::navigation
def->optional(properties.locationGraph.visuLocations,
"p.locationGraph.visuLocation",
"Enable visualization of locations.");
+
def->optional(properties.locationGraph.visuGraphEdges,
"p.locationGraph.visuGraphEdges",
"Enable visualization of navigation graph edges.");
- def->optional(properties.locationGraph.visuFrequency,
- "p.locationGraph.visuFrequency",
+
+ def->optional(properties.visuCostmaps,
+ "p.visuCostmaps",
+ "Enable visualization of costmaps.");
+
+ def->optional(properties.visuHumans,
+ "p.visuHumans",
+ "Enable visualization of humans.");
+
+ def->optional(properties.visuTransparent,
+ "p.visuTransparent",
+ "Enable visualization of humans a bit transparent.");
+
+ def->optional(properties.visuRooms,
+ "p.visuRooms",
+ "Enable visualization of rooms.");
+
+ def->optional(properties.visuFrequency,
+ "p.visuFrequency",
"Visualization frequeny of locations and graph edges [Hz].");
return def;
@@ -103,7 +118,7 @@ namespace armarx::navigation
void
- NavigationMemory::onInitComponent()
+ Component::onInitComponent()
{
// Topics and properties defined above are automagically registered.
@@ -118,13 +133,13 @@ namespace armarx::navigation
algorithms::arondto::Costmap::ToAronType());
workingMemory().addCoreSegment("Results_GlobalPlanner",
- navigation::core::arondto::Trajectory::ToAronType());
+ navigation::core::arondto::GlobalTrajectory::ToAronType());
workingMemory().addCoreSegment("Results_LocalPlanner",
- navigation::core::arondto::Trajectory::ToAronType());
- workingMemory().addCoreSegment("Results_TrajectoryController",
- navigation::core::arondto::Twist::ToAronType());
- workingMemory().addCoreSegment("Results_SafetyController",
- navigation::core::arondto::Twist::ToAronType());
+ navigation::core::arondto::LocalTrajectory::ToAronType());
+ // workingMemory().addCoreSegment("Results_TrajectoryController",
+ // navigation::core::arondto::Twist::ToAronType());
+ // workingMemory().addCoreSegment("Results_SafetyController",
+ // navigation::core::arondto::Twist::ToAronType());
workingMemory().addCoreSegment("Events"); //, armem::example::ExampleData::ToAronType());
// workingMemory.addCoreSegment("Exceptions"); //, armem::example::ExampleData::ToAronType());
@@ -135,6 +150,11 @@ namespace armarx::navigation
workingMemory().addCoreSegment(navigation::graph::coreSegmentID.coreSegmentName,
navigation::core::arondto::Graph::ToAronType());
+ workingMemory().addCoreSegment(memory::constants::HumanCoreSegmentName,
+ navigation::human::arondto::Human::ToAronType());
+
+ // workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
+ // navigation::human::arondto::Human::ToAronType());
workingMemory().addCoreSegment(navigation::rooms::coreSegmentID.coreSegmentName,
navigation::rooms::arondto::Room::ToAronType());
@@ -389,7 +409,7 @@ namespace armarx::navigation
void
- NavigationMemory::onConnectComponent()
+ Component::onConnectComponent()
{
// Do things after connecting to topics and components.
@@ -417,26 +437,32 @@ namespace armarx::navigation
void
- NavigationMemory::onDisconnectComponent()
+ Component::onDisconnectComponent()
{
}
void
- NavigationMemory::onExitComponent()
+ Component::onExitComponent()
{
}
std::string
- NavigationMemory::getDefaultName() const
+ Component::getDefaultName() const
{
- return "NavigationMemory";
+ return GetDefaultName();
+ }
+
+ std::string
+ Component::GetDefaultName()
+ {
+ return "navigation_memory";
}
void
- NavigationMemory::createRemoteGuiTab()
+ Component::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
@@ -451,14 +477,14 @@ namespace armarx::navigation
void
- NavigationMemory::RemoteGui_update()
+ Component::RemoteGui_update()
{
tab.locationGraph.update(*this);
}
void
- NavigationMemory::RemoteGuiTab::LocationGraph::setup(NavigationMemory& owner)
+ Component::RemoteGuiTab::LocationGraph::setup(Component& owner)
{
using namespace armarx::RemoteGui::Client;
GridLayout grid;
@@ -479,7 +505,7 @@ namespace armarx::navigation
void
- NavigationMemory::RemoteGuiTab::LocationGraph::update(NavigationMemory& owner)
+ Component::RemoteGuiTab::LocationGraph::update(Component& owner)
{
if (visuLocations.hasValueChanged() or visuGraphEdges.hasValueChanged())
{
@@ -491,41 +517,52 @@ namespace armarx::navigation
void
- NavigationMemory::visuRun()
+ Component::visuRun()
{
memory::Visu visu{arviz,
workingMemory().getCoreSegment(navigation::location::coreSegmentID),
workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
+ workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
+ workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName),
workingMemory().getCoreSegment(navigation::rooms::coreSegmentID)};
- Properties::LocationGraph p;
+ Properties::LocationGraph pp;
+ Properties p;
{
std::scoped_lock lock(propertiesMutex);
- p = properties.locationGraph;
+ pp = properties.locationGraph;
+ p = properties;
}
- CycleUtil cycle(static_cast<int>(1000 / p.visuFrequency));
+ Metronome metronome(Frequency::HertzDouble(p.visuFrequency));
while (tasks.visuTask and not tasks.visuTask->isStopped())
{
+ ARMARX_TRACE;
+
{
std::scoped_lock lock(propertiesMutex);
- p = properties.locationGraph;
+ pp = properties.locationGraph;
}
std::vector<viz::Layer> layers;
// Locations
- visu.drawLocations(layers, p.visuLocations);
+ visu.drawLocations(layers, pp.visuLocations);
// Graph Edges
- visu.drawGraphs(layers, p.visuGraphEdges);
+ visu.drawGraphs(layers, pp.visuGraphEdges);
+
+ // Costmaps
+ visu.drawCostmaps(layers, p.visuCostmaps);
+ // Humans
+ visu.drawHumans(layers, p.visuHumans, p.visuTransparent);
// Rooms
visu.drawRooms(layers, p.visuRooms);
arviz.commit(layers);
- cycle.waitForCycleDuration();
+ metronome.waitForNextTick();
}
}
@@ -644,7 +681,7 @@ namespace armarx::navigation
bool
- NavigationMemory::storeLocationGraph(const armarx::data::PackagePath& packagePath,
+ Component::storeLocationGraph(const armarx::data::PackagePath& packagePath,
const Ice::Current& current)
{
armem::server::wm::CoreSegment& locationCoreSegment =
@@ -716,4 +753,4 @@ namespace armarx::navigation
}
-} // namespace armarx::navigation
+} // namespace armarx::navigation::components::navigation_memory
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h b/source/armarx/navigation/components/navigation_memory/Component.h
similarity index 85%
rename from source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
rename to source/armarx/navigation/components/navigation_memory/Component.h
index 56809586a8b8c44ad917f5436abe5197cdb15ec3..77220afebd7eafae930ab19c7a436f35ee641d08 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
+++ b/source/armarx/navigation/components/navigation_memory/Component.h
@@ -22,23 +22,22 @@
#pragma once
+#include <mutex>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-
#include <ArmarXCore/interface/core/PackagePath.h>
+
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/server/plugins.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-
-#include <mutex>
-#include <armarx/navigation/components/NavigationMemory/ComponentInterface.h>
+#include <armarx/navigation/components/navigation_memory/ComponentInterface.h>
-namespace armarx::navigation
+namespace armarx::navigation::components::navigation_memory
{
/**
@@ -52,7 +51,7 @@ namespace armarx::navigation
*
* Detailed description of class NavigationMemory.
*/
- class NavigationMemory :
+ class Component :
virtual public armarx::Component,
// , virtual public armarx::DebugObserverComponentPluginUser
virtual public armarx::LightweightRemoteGuiComponentPluginUser,
@@ -64,8 +63,9 @@ namespace armarx::navigation
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
- bool storeLocationGraph(const armarx::data::PackagePath& packagePath, const Ice::Current& current) override;
-
+ static std::string GetDefaultName();
+ bool storeLocationGraph(const armarx::data::PackagePath& packagePath,
+ const Ice::Current& current) override;
protected:
@@ -103,9 +103,18 @@ namespace armarx::navigation
{
bool visuLocations = true;
bool visuGraphEdges = true;
- bool visuRooms = true;
- float visuFrequency = 2;
+
};
+
+ float visuFrequency = 2;
+
+ bool visuCostmaps = true;
+
+ bool visuHumans = true;
+ bool visuTransparent = false;
+
+ bool visuRooms = true;
+
LocationGraph locationGraph;
};
Properties properties;
@@ -122,8 +131,8 @@ namespace armarx::navigation
armarx::RemoteGui::Client::CheckBox visuLocations;
armarx::RemoteGui::Client::CheckBox visuGraphEdges;
- void setup(NavigationMemory& owner);
- void update(NavigationMemory& owner);
+ void setup(Component& owner);
+ void update(Component& owner);
};
LocationGraph locationGraph;
};
@@ -137,4 +146,4 @@ namespace armarx::navigation
Tasks tasks;
};
-} // namespace armarx::navigation
+} // namespace armarx::navigation::components::navigation_memory
diff --git a/source/armarx/navigation/components/NavigationMemory/ComponentInterface.ice b/source/armarx/navigation/components/navigation_memory/ComponentInterface.ice
similarity index 100%
rename from source/armarx/navigation/components/NavigationMemory/ComponentInterface.ice
rename to source/armarx/navigation/components/navigation_memory/ComponentInterface.ice
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
similarity index 55%
rename from source/armarx/navigation/components/NavigationMemory/Visu.cpp
rename to source/armarx/navigation/components/navigation_memory/Visu.cpp
index 23c8dbf36ac5f35c2f00bf22fcafa151f2a70ded..c450b9534bc6574c743bd1287f366d7d29f549f2 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -24,15 +24,20 @@
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/GlasbeyLUT.h>
+#include <SimoxUtility/color/cmaps/colormaps.h>
#include "RobotAPI/components/ArViz/Client/elements/Path.h"
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-#include "armarx/navigation/conversions/eigen.h"
+#include "armarx/navigation/algorithms/visualization.h"
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/graph/Visu.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/aron_conversions.h>
#include <armarx/navigation/rooms/aron/Room.aron.generated.h>
#include <armarx/navigation/rooms/aron_conversions.h>
@@ -43,10 +48,14 @@ namespace armarx::navigation::memory
Visu::Visu(viz::Client arviz,
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
+ const armem::server::wm::CoreSegment& costmapSegment,
+ const armem::server::wm::CoreSegment& humanSegment,
const armem::server::wm::CoreSegment& roomsSegment) :
arviz(arviz),
locSegment(locSegment),
graphSegment(graphSegment),
+ costmapSegment(costmapSegment),
+ humanSegment(humanSegment),
roomsSegment(roomsSegment),
visu(std::make_unique<graph::GraphVisu>())
{
@@ -126,6 +135,88 @@ namespace armarx::navigation::memory
}
}
+ namespace
+ {
+
+ inline void
+ visualize(const human::Humans& humans, viz::Layer& layer, const bool visuTransparent)
+ {
+
+ 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});
+
+
+ // cylinder.color(simox::Color::orange());
+ // cylinder.radius(300);
+ // layer.add(cylinder);
+
+
+ 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, visuTransparent ? 100 : 255));
+ layer.add(mmm);
+
+
+ core::Pose pose3d = conv::to3D(human.pose);
+ pose3d.translation() += Eigen::Vector3f{0, 0, 1000};
+ auto arrow = viz::Arrow(std::to_string(layer.size()))
+ .pose(pose3d)
+ .length(200)
+ .color(simox::Color::red());
+ layer.add(arrow);
+ }
+ }
+
+ } // namespace
+
+ void
+ Visu::drawCostmaps(std::vector<viz::Layer>& layers, bool enabled)
+ {
+ if (not enabled)
+ {
+ return;
+ }
+
+ std::map<std::string, std::vector<std::pair<std::string, algorithms::Costmap>>>
+ namedProviderCostmaps;
+
+ costmapSegment.doLocked(
+ [&]()
+ {
+ using namespace armem::server;
+
+ costmapSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ if (const wm::EntityInstance* instance = entity.findLatestInstance())
+ {
+ navigation::algorithms::Costmap costmap =
+ algorithms::fromAron(*instance);
+
+ namedProviderCostmaps[instance->id().providerSegmentName].emplace_back(
+ instance->id().entityName, std::move(costmap));
+ }
+ });
+ });
+
+ for (const auto& [providerName, namedCostmaps] : namedProviderCostmaps)
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer("costmaps_" + providerName));
+ for (const auto& [name, costmap] : namedCostmaps)
+ {
+ algorithms::visualize(costmap, layer, name);
+ }
+ }
+ }
+
void
Visu::drawRooms(std::vector<viz::Layer>& layers, bool enabled)
@@ -191,5 +282,49 @@ namespace armarx::navigation::memory
layer.add(path);
}
+ void
+ Visu::drawHumans(std::vector<viz::Layer>& layers,
+ const bool enabled,
+ const bool visuTransparent)
+ {
+ if (not enabled)
+ {
+ return;
+ }
+
+ std::map<std::string, navigation::human::Humans> namedProviderHumans;
+
+ humanSegment.doLocked(
+ [&]()
+ {
+ using namespace armem::server;
+
+ humanSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ namedProviderHumans[entity.id().providerSegmentName];
+ entity.getLatestSnapshot().forEachInstance(
+ [&namedProviderHumans](
+ const armarx::armem::wm::EntityInstance& instance)
+ {
+ const auto dto =
+ navigation::human::arondto::Human::FromAron(instance.data());
+
+ navigation::human::Human human;
+ fromAron(dto, human);
+
+ namedProviderHumans[instance.id().providerSegmentName].emplace_back(
+ std::move(human));
+ });
+ });
+ });
+
+ for (const auto& [providerName, humans] : namedProviderHumans)
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
+ visualize(humans, layer, visuTransparent);
+ }
+ }
+
} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.h b/source/armarx/navigation/components/navigation_memory/Visu.h
similarity index 76%
rename from source/armarx/navigation/components/NavigationMemory/Visu.h
rename to source/armarx/navigation/components/navigation_memory/Visu.h
index 4389ed1d5a4c1f9cb4ea8cf4540ed70f16ba3a15..86515aa8987281682ec7125ad024d99f0fea4bc7 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.h
+++ b/source/armarx/navigation/components/navigation_memory/Visu.h
@@ -25,9 +25,13 @@
#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>
+#include <armarx/navigation/algorithms/Costmap.h>
+
#include <armarx/navigation/rooms/types.h>
namespace armarx::navigation::graph
@@ -44,21 +48,26 @@ namespace armarx::navigation::memory
Visu(viz::Client arviz,
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
+ const armem::server::wm::CoreSegment& costmapSegment,
+ const armem::server::wm::CoreSegment& humanSegment,
const armem::server::wm::CoreSegment& roomsSegment);
~Visu();
void drawLocations(std::vector<viz::Layer>& layers, bool enabled);
void drawGraphs(std::vector<viz::Layer>& layers, bool enabled);
+ void drawCostmaps(std::vector<viz::Layer>& layers, bool enabled);
+ void drawHumans(std::vector<viz::Layer>& layers, bool enabled, bool visuTransparent);
void drawRooms(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;
+ const armem::server::wm::CoreSegment& costmapSegment;
+ const armem::server::wm::CoreSegment& humanSegment;
const armem::server::wm::CoreSegment& roomsSegment;
std::unique_ptr<navigation::graph::GraphVisu> visu;
diff --git a/source/armarx/navigation/components/navigation_skill_provider/Component.cpp b/source/armarx/navigation/components/navigation_skill_provider/Component.cpp
index 3571e902ceb65ff75faed664af9ad05b2c0265c0..aebc3d0c2d51b523da61393e423849062ec8f1b0 100644
--- a/source/armarx/navigation/components/navigation_skill_provider/Component.cpp
+++ b/source/armarx/navigation/components/navigation_skill_provider/Component.cpp
@@ -25,9 +25,9 @@
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
-#include "armarx/navigation/skills/GuideHumanToRoom.h"
-#include "armarx/navigation/skills/NavigateTo.h"
-#include "armarx/navigation/skills/NavigateToLocation.h"
+#include <armarx/navigation/skills/GuideHumanToRoom.h>
+#include <armarx/navigation/skills/NavigateTo.h>
+#include <armarx/navigation/skills/NavigateToLocation.h>
// Include headers you only need in function definitions in the .cpp.
@@ -85,6 +85,10 @@ namespace armarx::navigation::components::navigation_skill_provider
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
+
+ skills_.navigateTo = addSkill<skills::NavigateTo>();
+ skills_.navigateToLocation = addSkill<skills::NavigateToLocation>();
+ skills_.guideHumanToRoom = addSkill<skills::GuideHumanToRoom>();
}
@@ -97,25 +101,36 @@ namespace armarx::navigation::components::navigation_skill_provider
{
- skills::NavigateTo::Context ctx{.iceNavigator = iceNavigator,
- .memoryNameSystem = memoryNameSystem()};
- addSkill(std::make_unique<skills::NavigateTo>(ctx));
+ ARMARX_CHECK_NOT_NULL(skills_.navigateTo);
+
+ skills::NavigateTo::Services srv{.iceNavigator = iceNavigator,
+ .memoryNameSystem = memoryNameSystem()};
+ skills_.navigateTo->connect(srv);
}
{
- skills::NavigateToLocation::Context ctx{.iceNavigator = iceNavigator,
- .memoryNameSystem = memoryNameSystem()};
- addSkill(std::make_unique<skills::NavigateToLocation>(ctx));
+ ARMARX_CHECK_NOT_NULL(skills_.navigateToLocation);
+
+ skills::NavigateToLocation::Services srv{.iceNavigator = iceNavigator,
+ .memoryNameSystem = memoryNameSystem()};
+ skills_.navigateToLocation->connect(srv);
}
{
- skills::GuideHumanToRoom::Context ctx{.iceNavigator = iceNavigator,
- .memoryNameSystem = memoryNameSystem(),
- .virtualRobotReader =
- virtualRobotReaderPlugin->get(),
- .costmapReader = costmapReaderPlugin->get(),
- .roomsReader = roomsReaderPlugin->get()};
- addSkill(std::make_unique<skills::GuideHumanToRoom>(ctx));
+ ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
+ ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
+ ARMARX_CHECK_NOT_NULL(roomsReaderPlugin);
+
+ ARMARX_CHECK_NOT_NULL(skills_.guideHumanToRoom);
+
+ skills::GuideHumanToRoom::Services srv{.iceNavigator = iceNavigator,
+ .memoryNameSystem = memoryNameSystem(),
+ .virtualRobotReader =
+ virtualRobotReaderPlugin->get(),
+ .costmapReader = costmapReaderPlugin->get(),
+ .roomsReader = roomsReaderPlugin->get(),
+ .arviz = arviz};
+ skills_.guideHumanToRoom->connect(srv);
}
diff --git a/source/armarx/navigation/components/navigation_skill_provider/Component.h b/source/armarx/navigation/components/navigation_skill_provider/Component.h
index e620c8ceb9cd9dc68032852979e347f94dffcd64..bcadb0bc43686bf8633ce675943881e9b9e77993 100644
--- a/source/armarx/navigation/components/navigation_skill_provider/Component.h
+++ b/source/armarx/navigation/components/navigation_skill_provider/Component.h
@@ -23,28 +23,27 @@
#pragma once
-
-// #include <mutex>
-
#include <ArmarXCore/core/Component.h>
-// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-
-// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
-// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-
-#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.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/skills/provider/SkillProviderComponentPlugin.h>
-#include "armarx/navigation/client/services/IceNavigator.h"
-#include "armarx/navigation/memory/client/costmap/Reader.h"
-#include "armarx/navigation/memory/client/rooms/Reader.h"
#include <armarx/navigation/client.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
+#include <armarx/navigation/client/services/IceNavigator.h>
#include <armarx/navigation/components/navigation_skill_provider/ComponentInterface.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/memory/client/rooms/Reader.h>
+
+namespace armarx::navigation::skills
+{
+ class NavigateTo;
+ class NavigateToLocation;
+ class GuideHumanToRoom;
+} // namespace armarx::navigation::skills
namespace armarx::navigation::components::navigation_skill_provider
{
@@ -54,14 +53,12 @@ namespace armarx::navigation::components::navigation_skill_provider
virtual public ComponentInterface,
virtual public armarx::SkillProviderComponentPluginUser,
// virtual public armarx::navigation::client::ComponentPluginUser,
- virtual public armem::ListeningClientPluginUser
-
- // , virtual public armarx::DebugObserverComponentPluginUser
- // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser,
+ virtual public armem::ListeningClientPluginUser,
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ virtual public armarx::ArVizComponentPluginUser
{
public:
-
Component();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -110,6 +107,13 @@ namespace armarx::navigation::components::navigation_skill_provider
void drawBoxes(const Properties& p, viz::Client& arviz);
*/
+ struct Skills
+ {
+ skills::NavigateTo* navigateTo;
+ skills::NavigateToLocation* navigateToLocation;
+ skills::GuideHumanToRoom* guideHumanToRoom;
+ } skills_;
+
private:
static const std::string defaultName;
@@ -158,7 +162,8 @@ namespace armarx::navigation::components::navigation_skill_provider
using ReaderWriterPlugin = armem::client::plugins::ReaderWriterPlugin<T>;
- ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin = nullptr;
+ ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin =
+ nullptr;
ReaderWriterPlugin<memory::client::costmap::Reader>* costmapReaderPlugin = nullptr;
ReaderWriterPlugin<memory::client::rooms::Reader>* roomsReaderPlugin = nullptr;
};
diff --git a/source/armarx/navigation/components/Navigator/CMakeLists.txt b/source/armarx/navigation/components/navigator/CMakeLists.txt
similarity index 66%
rename from source/armarx/navigation/components/Navigator/CMakeLists.txt
rename to source/armarx/navigation/components/navigator/CMakeLists.txt
index 57f641b3d5aa40db997bf83f10a83b849fd981b0..a686acda88e040fba889f7d0c0ee4bbafa28eaa4 100644
--- a/source/armarx/navigation/components/Navigator/CMakeLists.txt
+++ b/source/armarx/navigation/components/navigator/CMakeLists.txt
@@ -1,14 +1,15 @@
armarx_add_component(navigator
SOURCES
- Navigator.cpp
+ Component.cpp
RemoteGui.cpp
HEADERS
- Navigator.h
+ Component.h
RemoteGui.h
DEPENDENCIES
# ArmarXCore
ArmarXCore
- # ArmarXCoreComponentPlugins # For DebugObserver plugin. ArmarXGui
+ ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ # ArmarXGui
ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
RobotAPICore
@@ -18,15 +19,13 @@ armarx_add_component(navigator
armem_robot
armem_robot_state
armem_vision
- # This project ${PROJECT_NAME}Interfaces # For ice interfaces from this
- # package. This component
+ # navigation
armarx_navigation::server
armarx_navigation::client
armarx_navigation::util
armarx_navigation::factories
+ # Simox
Simox::SimoxUtility
+ # others
Eigen3::Eigen
- ICE_DEPENDENCIES
- ArmarXCoreInterfaces
- RobotAPIInterfaces
)
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/navigator/Component.cpp
similarity index 68%
rename from source/armarx/navigation/components/Navigator/Navigator.cpp
rename to source/armarx/navigation/components/navigator/Component.cpp
index 707388290a95579751196b7ff7da3621e6265515..3943986f1cccd1496240f844b0d3f08a8f714478 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -21,7 +21,7 @@
* GNU General Public License
*/
-#include "Navigator.h"
+#include "Component.h"
#include <algorithm>
#include <cmath>
@@ -29,6 +29,7 @@
#include <cstdint>
#include <iterator>
#include <memory>
+#include <optional>
#include <utility>
#include <Eigen/Geometry>
@@ -56,20 +57,16 @@
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
-#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
-#include <RobotAPI/components/ArViz/Client/Elements.h>
-#include <RobotAPI/components/ArViz/Client/elements/Color.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-#include <RobotAPI/interface/ArViz/Elements.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include "armarx/navigation/server/scene_provider/SceneProvider.h"
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/algorithms/astar/util.h>
@@ -80,18 +77,21 @@
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/client/ice_conversions.h>
-#include <armarx/navigation/components/Navigator/RemoteGui.h>
+#include <armarx/navigation/components/navigator/RemoteGui.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/factories/NavigationStackFactory.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
+#include <armarx/navigation/server/execution/ExecutorInterface.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
+#include <armarx/navigation/server/scene_provider/SceneProvider.h>
#include <armarx/navigation/util/util.h>
-namespace armarx::navigation::components
+namespace armarx::navigation::components::navigator
{
- ARMARX_DECOUPLED_REGISTER_COMPONENT(Navigator);
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
}
namespace armarx::navigation
@@ -111,10 +111,10 @@ namespace armarx::navigation
} // namespace armarx::navigation
-namespace armarx::navigation::components
+namespace armarx::navigation::components::navigator
{
- components::Navigator::Navigator() : parameterizationService(nullptr, nullptr)
+ Component::Component() : parameterizationService(nullptr, nullptr)
// publisher(&resultsWriter, &eventsWriter)
{
// scene().timeServer = &timeServer;
@@ -125,7 +125,7 @@ namespace armarx::navigation::components
addPlugin(resultsWriterPlugin);
addPlugin(graphReaderPlugin);
addPlugin(costmapReaderPlugin);
- addPlugin(costmapWriterPlugin);
+ addPlugin(humanReaderPlugin);
addPlugin(virtualRobotReaderPlugin);
@@ -134,15 +134,15 @@ namespace armarx::navigation::components
}
- components::Navigator::~Navigator() = default;
+ Component::~Component() = default;
void
- components::Navigator::onInitComponent()
+ Component::onInitComponent()
{
}
void
- components::Navigator::onConnectComponent()
+ Component::onConnectComponent()
{
ARMARX_TRACE;
@@ -167,29 +167,27 @@ namespace armarx::navigation::components
.graphReader = &graphReaderPlugin->get(),
.costmapReader = &costmapReaderPlugin->get(),
.virtualRobotReader = &virtualRobotReaderPlugin->get(),
+ .humanReader = &humanReaderPlugin->get(),
.objectPoseClient = ObjectPoseClientPluginUser::getClient()};
- const std::string robotName = getControlComponentPlugin()
- .getRobotUnitPlugin()
- .getRobotUnit()
- ->getKinematicUnit()
- ->getRobotName();
-
- const server::scene_provider::SceneProvider::Config cfg{.robotName = robotName};
- sceneProvider = server::scene_provider::SceneProvider(srv, cfg);
+ sceneProvider.emplace(srv, params.sceneCfg);
}
initializeScene();
ARMARX_TRACE;
- executor.emplace(server::PlatformControllerExecutor(getControlComponentPlugin()));
+
+ if (not params.disableExecutor)
+ {
+ executor.emplace(server::PlatformControllerExecutor(getControlComponentPlugin()));
+ }
ARMARX_TRACE;
introspector = server::ArvizIntrospector(getArvizClient(), scene().robot);
// memoryIntrospector = server::MemoryIntrospector(resultsWriterPlugin->get(), );
- navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
+ navRemoteGui.emplace(remoteGui, *this);
navRemoteGui->enable();
initialized = true;
@@ -198,14 +196,14 @@ namespace armarx::navigation::components
}
void
- components::Navigator::onReconnectComponent()
+ Component::onReconnectComponent()
{
ARMARX_TRACE;
navRemoteGui->enable();
}
void
- components::Navigator::onDisconnectComponent()
+ Component::onDisconnectComponent()
{
ARMARX_TRACE;
@@ -214,27 +212,34 @@ namespace armarx::navigation::components
}
void
- components::Navigator::onExitComponent()
+ Component::onExitComponent()
{
}
void
- components::Navigator::initializeScene()
+ Component::initializeScene()
{
ARMARX_TRACE;
sceneProvider->initialize(armarx::Clock::Now());
}
std::string
- components::Navigator::getDefaultName() const
+ Component::getDefaultName() const
{
- return "Navigator";
+ return GetDefaultName();
}
+ std::string
+ Component::GetDefaultName()
+ {
+ return "navigator";
+ }
+
+
void
- components::Navigator::createConfig(const aron::data::dto::DictPtr& stackConfig,
- const std::string& callerId,
- const Ice::Current&)
+ Component::createConfig(const aron::data::dto::DictPtr& stackConfig,
+ const std::string& callerId,
+ const Ice::Current&)
{
// TODO: Not thread-safe.
ARMARX_TRACE;
@@ -245,6 +250,12 @@ namespace armarx::navigation::components
server::NavigationStack stack =
fac::NavigationStackFactory::create(stackConfig, sceneProvider->scene());
+ // set visualization of LocalPlanner
+ if (stack.localPlanner)
+ {
+ stack.localPlanner->setVisualization(getArvizClient());
+ }
+
memoryIntrospectors.emplace_back(
std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
@@ -254,24 +265,31 @@ namespace armarx::navigation::components
std::make_unique<server::MemoryPublisher>(
&resultsWriterPlugin->get(), &eventsWriterPlugin->get(), callerId));
+ server::ExecutorInterface* executorPtr = nullptr;
+ if (executor.has_value())
+ {
+ executorPtr = &executor.value();
+ }
+
navigators.emplace(
std::piecewise_construct,
std::forward_as_tuple(callerId),
std::forward_as_tuple(
server::Navigator::Config{.stack = std::move(stack),
.general = server::Navigator::Config::General{}},
- server::Navigator::InjectedServices{.executor = &executor.value(),
- .publisher =
- memoryPublishers.at(callerId).get(),
- .introspector = &(introspector.value()),
- .sceneProvider = &sceneProvider.value()}));
+ server::Navigator::InjectedServices{
+ .executor = executorPtr,
+ .publisher = memoryPublishers.at(callerId).get(),
+ .introspector = &(introspector.value()),
+ .debugObserverHelper = &getDebugObserverComponentPlugin(),
+ .sceneProvider = &sceneProvider.value()}));
}
void
- components::Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
- const std::string& navigationMode,
- const std::string& callerId,
- const Ice::Current&)
+ Component::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
+ const std::string& navigationMode,
+ const std::string& callerId,
+ const Ice::Current&)
{
ARMARX_TRACE;
@@ -296,9 +314,40 @@ namespace armarx::navigation::components
}
}
+ void
+ Component::updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
+ const std::string& navigationMode,
+ const std::string& callerId,
+ const Ice::Current& /*c*/)
+ {
+ ARMARX_TRACE;
+
+ ARMARX_INFO << "updateMoveTo() requested by caller '" << callerId << "'";
+ ARMARX_CHECK(navigators.count(callerId) > 0)
+ << "Navigator config for caller `" << callerId << "` not registered!";
+
+ const auto checkIsActiveNavigator = [&](const std::string& callerId)
+ {
+ const std::optional<std::string> activeNavigatorCallerId = activeNavigatorId();
+ if (not activeNavigatorCallerId.has_value())
+ {
+ ARMARX_VERBOSE << "No navigator is active.";
+ return false;
+ }
+
+ return activeNavigatorCallerId == callerId;
+ };
+
+ ARMARX_CHECK(checkIsActiveNavigator(callerId))
+ << "The navigator with id `" << callerId << "` is not active!";
+
+ navigators.at(callerId).update(convert(waypoints),
+ core::NavigationFrameNames.from_name(navigationMode));
+ }
+
void
- Navigator::moveTo2(const client::detail::Waypoints& waypoints,
+ Component::moveTo2(const client::detail::Waypoints& waypoints,
const std::string& navigationMode,
const std::string& callerId,
const Ice::Current& c)
@@ -314,12 +363,25 @@ namespace armarx::navigation::components
navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
}
+
+ void Component::moveToLocation(const std::string& location,
+ const std::string& callerId,
+ const Ice::Current& c)
+ {
+ ARMARX_TRACE;
+ ARMARX_INFO << "MoveToLocation `" << location << "` requested by caller '" << callerId << "'";
+
+ ARMARX_CHECK(navigators.count(callerId) > 0)
+ << "Navigator config for caller `" << callerId << "` not registered!";
+
+ navigators.at(callerId).moveToLocation(location);
+ }
void
- components::Navigator::moveTowards(const Eigen::Vector3f& direction,
- const std::string& navigationMode,
- const std::string& callerId,
- const Ice::Current&)
+ Component::moveTowards(const Eigen::Vector3f& direction,
+ const std::string& navigationMode,
+ const std::string& callerId,
+ const Ice::Current&)
{
// TODO: Error handling.
ARMARX_TRACE;
@@ -333,7 +395,7 @@ namespace armarx::navigation::components
}
void
- components::Navigator::pause(const std::string& configId, const Ice::Current&)
+ Component::pause(const std::string& configId, const Ice::Current&)
{
ARMARX_CHECK(navigators.count(configId) > 0)
<< "Navigator config for caller `" << configId << "` not registered!";
@@ -341,7 +403,7 @@ namespace armarx::navigation::components
}
void
- components::Navigator::resume(const std::string& configId, const Ice::Current&)
+ Component::resume(const std::string& configId, const Ice::Current&)
{
ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
@@ -350,7 +412,7 @@ namespace armarx::navigation::components
}
void
- components::Navigator::stop(const std::string& configId, const Ice::Current&)
+ Component::stop(const std::string& configId, const Ice::Current&)
{
// FIXME make sure that event is emitted
ARMARX_TRACE;
@@ -365,7 +427,7 @@ namespace armarx::navigation::components
}
void
- components::Navigator::stopAll(const Ice::Current&)
+ Component::stopAll(const Ice::Current&)
{
ARMARX_IMPORTANT << "stopAll()";
@@ -385,7 +447,7 @@ namespace armarx::navigation::components
}
bool
- components::Navigator::isPaused(const std::string& configId, const Ice::Current&)
+ Component::isPaused(const std::string& configId, const Ice::Current&)
{
ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
@@ -394,14 +456,14 @@ namespace armarx::navigation::components
}
const core::Scene&
- Navigator::scene() const
+ Component::scene() const
{
ARMARX_CHECK_NOT_NULL(sceneProvider);
return sceneProvider->scene();
}
bool
- components::Navigator::isStopped(const std::string& configId, const Ice::Current&)
+ Component::isStopped(const std::string& configId, const Ice::Current&)
{
ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
@@ -410,7 +472,7 @@ namespace armarx::navigation::components
}
armarx::PropertyDefinitionsPtr
- components::Navigator::createPropertyDefinitions()
+ Component::createPropertyDefinitions()
{
ARMARX_TRACE;
@@ -433,6 +495,16 @@ namespace armarx::navigation::components
"Threshold for each cell to be considered occupied. Increase this value to "
"reduce noise.");
+ def->optional(params.disableExecutor,
+ "p.disableExecutor",
+ "If the executor is disabled, the navigator will only plan the trajectory "
+ "but won't execute it.");
+
+ def->required(params.sceneCfg.robotName, "p.scene.robotName");
+ def->optional(params.sceneCfg.staticCostmapProviderName, "p.scene.staticCostmapProviderName");
+ def->optional(params.sceneCfg.staticCostmapName, "p.scene.staticCostmapName");
+ def->optional(params.sceneCfg.humanProviderName, "p.scene.humanProviderName");
+
return def;
}
@@ -478,25 +550,38 @@ namespace armarx::navigation::components
}
server::Navigator*
- components::Navigator::activeNavigator()
+ Component::activeNavigator()
+ {
+
+ const auto navigatorId = activeNavigatorId();
+ if (not navigatorId.has_value())
+ {
+ return nullptr;
+ }
+
+ return &navigators.at(navigatorId.value());
+ }
+
+ std::optional<std::string>
+ Component::activeNavigatorId() const
{
ARMARX_TRACE;
// We define the active navigator to be the one whose movement is enabled.
- const auto isActive = [](auto& nameNavPair) -> bool
+ const auto isActive = [](const auto& nameNavPair) -> bool
{
- server::Navigator& navigator = nameNavPair.second;
+ const auto& [name, navigator] = nameNavPair;
return not navigator.isPaused();
};
- auto it = std::find_if(navigators.begin(), navigators.end(), isActive);
+ const auto it = std::find_if(navigators.begin(), navigators.end(), isActive);
// no navigator active?
if (it == navigators.end())
{
- return nullptr;
+ return std::nullopt;
}
- return &it->second;
+ return it->first;
}
-} // namespace armarx::navigation::components
+} // namespace armarx::navigation::components::navigator
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/navigator/Component.h
similarity index 86%
rename from source/armarx/navigation/components/Navigator/Navigator.h
rename to source/armarx/navigation/components/navigator/Component.h
index 7c725c3197c3774980dabdfda27115b5cf5db64d..aac593f1e2ef8bb2bf405bfa2dff65cffeff3e58 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/navigator/Component.h
@@ -33,6 +33,7 @@
#include <Ice/Object.h>
+#include "ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
@@ -42,7 +43,6 @@
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
@@ -51,10 +51,10 @@
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include <armarx/control/client/ComponentPlugin.h>
-#include <armarx/navigation/client/ice/NavigatorInterface.h>
-#include <armarx/navigation/components/Navigator/RemoteGui.h>
+
+#include <armarx/navigation/memory/client/human/Reader.h>
+#include <armarx/navigation/components/navigator/RemoteGui.h>
#include <armarx/navigation/core/types.h>
-#include <armarx/navigation/memory/client/costmap/Writer.h>
#include <armarx/navigation/memory/client/graph/Reader.h>
#include <armarx/navigation/memory/client/parameterization/Reader.h>
#include <armarx/navigation/memory/client/stack_result/Writer.h>
@@ -67,7 +67,7 @@
#include <armarx/navigation/server/scene_provider/SceneProvider.h>
-namespace armarx::navigation::components
+namespace armarx::navigation::components::navigator
{
/**
@@ -81,22 +81,25 @@ namespace armarx::navigation::components
*
* Detailed description of class Navigator.
*/
- class Navigator :
- virtual public Component,
+ class Component :
+ virtual public armarx::Component,
virtual public client::NavigatorInterface,
virtual public ObjectPoseClientPluginUser,
virtual public ArVizComponentPluginUser,
- virtual public armarx::control::client::ComponentPluginUser
+ virtual public armarx::control::client::ComponentPluginUser,
+ virtual public armarx::DebugObserverComponentPluginUser
// virtual public armem::ListeningClientPluginUser
{
public:
- Navigator();
- ~Navigator() override;
+ Component();
+ ~Component() override;
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+ static std::string GetDefaultName();
+
void createConfig(const aron::data::dto::DictPtr& stackConfig,
const std::string& callerId,
const Ice::Current& c = Ice::emptyCurrent) override;
@@ -111,6 +114,15 @@ namespace armarx::navigation::components
const std::string& callerId,
const Ice::Current& c = Ice::emptyCurrent) override;
+ void moveToLocation(const std::string& location,
+ const std::string& callerId,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+
+ void updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
+ const std::string& navigationMode,
+ const std::string& callerId,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+
void moveTowards(const Eigen::Vector3f& direction,
const std::string& navigationMode,
const std::string& callerId,
@@ -158,6 +170,7 @@ namespace armarx::navigation::components
// void updateRobot();
server::Navigator* activeNavigator();
+ std::optional<std::string> activeNavigatorId() const;
private:
void visualizeSPFA();
@@ -175,9 +188,7 @@ namespace armarx::navigation::components
std::mutex propertiesMutex;
- // TODO maybe as optional, but requires some effort
- std::unique_ptr<NavigatorRemoteGui> navRemoteGui;
-
+ std::optional<armarx::navigation::components::navigator::RemoteGui> navRemoteGui;
// unique_ptr to avoid dangling refs
std::vector<std::unique_ptr<server::MemoryIntrospector>> memoryIntrospectors;
@@ -195,8 +206,9 @@ namespace armarx::navigation::components
graphReaderPlugin = nullptr;
armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Reader>*
costmapReaderPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Writer>*
- costmapWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::human::Reader>*
+ humanReaderPlugin = nullptr;
+
// armem::vision::occupancy_grid::client::Reader occupancyGridReader;
// `robot_state` memory reader and writer
@@ -214,8 +226,11 @@ namespace armarx::navigation::components
struct Parameters
{
float occupiedGridThreshold{0.55F};
+ bool disableExecutor{false};
+
+ server::scene_provider::SceneProvider::Config sceneCfg;
};
Parameters params;
};
-} // namespace armarx::navigation::components
+} // namespace armarx::navigation::components::navigator
diff --git a/source/armarx/navigation/components/Navigator/RemoteGui.cpp b/source/armarx/navigation/components/navigator/RemoteGui.cpp
similarity index 92%
rename from source/armarx/navigation/components/Navigator/RemoteGui.cpp
rename to source/armarx/navigation/components/navigator/RemoteGui.cpp
index 9ab923ba04998de737a9513b59e789ee3bc33b4a..42d9ad21404eb682d28809569947ea165337f33b 100644
--- a/source/armarx/navigation/components/Navigator/RemoteGui.cpp
+++ b/source/armarx/navigation/components/navigator/RemoteGui.cpp
@@ -15,25 +15,25 @@
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include "Navigator.h"
-#include <armarx/navigation/global_planning/SPFA.h>
+#include "Component.h"
+#include <armarx/navigation/local_planning/TimedElasticBandsParams.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/factories/NavigationStackFactory.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/Point2Point.h>
+#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/server/Navigator.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
#include <armarx/navigation/util/util.h>
-namespace armarx::navigation::components
+namespace armarx::navigation::components::navigator
{
namespace gui = RemoteGui;
- NavigatorRemoteGui::NavigatorRemoteGui(const RemoteGuiInterfacePrx& remoteGui,
- Navigator& navigator) :
+ RemoteGui::RemoteGui(const RemoteGuiInterfacePrx& remoteGui, Component& navigator) :
remoteGui(remoteGui),
- runningTask(new RunningTask<NavigatorRemoteGui>(this, &NavigatorRemoteGui::run)),
+ runningTask(new RunningTask<RemoteGui>(this, &RemoteGui::run)),
navigator(navigator)
{
createRemoteGuiTab();
@@ -43,13 +43,13 @@ namespace armarx::navigation::components
runningTask->start();
}
- NavigatorRemoteGui::~NavigatorRemoteGui()
+ RemoteGui::~RemoteGui()
{
shutdown();
}
void
- NavigatorRemoteGui::createRemoteGuiTab()
+ RemoteGui::createRemoteGuiTab()
{
ARMARX_TRACE;
@@ -192,7 +192,7 @@ namespace armarx::navigation::components
}
void
- NavigatorRemoteGui::run()
+ RemoteGui::run()
{
constexpr int kCycleDurationMs = 100;
@@ -226,11 +226,11 @@ namespace armarx::navigation::components
}
void
- NavigatorRemoteGui::handleEvents(RemoteGui::TabProxy& tab3)
+ RemoteGui::handleEvents(::armarx::RemoteGui::TabProxy& tab3)
{
ARMARX_TRACE;
- const std::string configId = "NavigatorRemoteGui";
+ const std::string configId = "RemoteGui";
if (tab.controlGroup.moveToButton.wasClicked())
{
@@ -254,13 +254,14 @@ namespace armarx::navigation::components
client::NavigationStackConfig cfg;
cfg.globalPlanner(global_planning::SPFAParams());
- cfg.trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams());
+ cfg.localPlanner(local_planning::TimedElasticBandsParams());
+ cfg.trajectoryController(traj_ctrl::local::TrajectoryFollowingControllerParams());
const auto stackConfig = cfg.toAron();
ARMARX_TRACE;
- navigator.createConfig(stackConfig, "NavigatorRemoteGui");
+ navigator.createConfig(stackConfig, "RemoteGui");
const Eigen::Vector3f targetPos{tab.targetPoseGroup.targetPoseX.getValue(),
tab.targetPoseGroup.targetPoseY.getValue(),
@@ -319,7 +320,7 @@ namespace armarx::navigation::components
}
void
- NavigatorRemoteGui::shutdown()
+ RemoteGui::shutdown()
{
// std::lock_guard g{mtx};
@@ -333,7 +334,7 @@ namespace armarx::navigation::components
}
void
- NavigatorRemoteGui::enable()
+ RemoteGui::enable()
{
ARMARX_TRACE;
@@ -346,7 +347,7 @@ namespace armarx::navigation::components
}
void
- NavigatorRemoteGui::disable()
+ RemoteGui::disable()
{
ARMARX_TRACE;
@@ -364,7 +365,7 @@ namespace armarx::navigation::components
}
void
- NavigatorRemoteGui::reset()
+ RemoteGui::reset()
{
ARMARX_TRACE;
@@ -381,4 +382,4 @@ namespace armarx::navigation::components
tab.sendUpdates();
}
-} // namespace armarx::navigation::components
+} // namespace armarx::navigation::components::navigator
diff --git a/source/armarx/navigation/components/Navigator/RemoteGui.h b/source/armarx/navigation/components/navigator/RemoteGui.h
similarity index 86%
rename from source/armarx/navigation/components/Navigator/RemoteGui.h
rename to source/armarx/navigation/components/navigator/RemoteGui.h
index ee8b78b60c65ae9dc29c79ed9ad963f1c469cd50..2cdc30c5919f15e5b7c761809711860504250e8c 100644
--- a/source/armarx/navigation/components/Navigator/RemoteGui.h
+++ b/source/armarx/navigation/components/navigator/RemoteGui.h
@@ -33,16 +33,16 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Tab.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-namespace armarx::navigation::components
+namespace armarx::navigation::components::navigator
{
- class Navigator;
+ class Component;
- class NavigatorRemoteGui
+ class RemoteGui
{
public:
- NavigatorRemoteGui(const RemoteGuiInterfacePrx& remoteGui, Navigator& navigator);
+ RemoteGui(const RemoteGuiInterfacePrx& remoteGui, Component& navigator);
- ~NavigatorRemoteGui();
+ ~RemoteGui();
void shutdown();
@@ -57,18 +57,18 @@ namespace armarx::navigation::components
std::mutex handleEventsMtx;
RemoteGuiInterfacePrx remoteGui;
- RunningTask<NavigatorRemoteGui>::pointer_type runningTask;
+ RunningTask<RemoteGui>::pointer_type runningTask;
- Navigator& navigator;
+ Component& navigator;
- RemoteGui::TabProxy tabPrx;
+ armarx::RemoteGui::TabProxy tabPrx;
void createRemoteGuiTab();
// thread function
void run();
- void handleEvents(RemoteGui::TabProxy& tab);
+ void handleEvents(armarx::RemoteGui::TabProxy& tab);
const std::string REMOTE_GUI_TAB_MAME = "Navigator";
@@ -105,4 +105,4 @@ namespace armarx::navigation::components
RemoteGuiTab tab;
};
-} // namespace armarx::navigation::components
+} // namespace armarx::navigation::components::navigator
diff --git a/source/armarx/navigation/core/CMakeLists.txt b/source/armarx/navigation/core/CMakeLists.txt
index 4ee7e461279c24d83a67e7739b2df3f17ae1a822..d95cd0ed7c210e76675b2a9f286ccdd0854f6401 100644
--- a/source/armarx/navigation/core/CMakeLists.txt
+++ b/source/armarx/navigation/core/CMakeLists.txt
@@ -13,6 +13,7 @@ armarx_add_library(core
SOURCES
StaticScene.cpp
Trajectory.cpp
+ basic_types.cpp
types.cpp
Graph.cpp
aron_conversions.cpp
diff --git a/source/armarx/navigation/core/DynamicScene.h b/source/armarx/navigation/core/DynamicScene.h
index 05f211a9a6a4d40e243ab560a437814d3eb02b94..172957d00e1adb5b42a1bb47ca9fea7f80083b17 100644
--- a/source/armarx/navigation/core/DynamicScene.h
+++ b/source/armarx/navigation/core/DynamicScene.h
@@ -23,17 +23,14 @@
#pragma once
+#include <armarx/navigation/human/types.h>
+
namespace armarx::navigation::core
{
struct DynamicScene
{
-
- // TODO(SALt): Implement
-
- public:
- protected:
- private:
+ human::Humans humans;
};
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/Graph.h b/source/armarx/navigation/core/Graph.h
index 904f51c69913e698c064c0edf0debcf9348fdf02..92097dff9048fef9d649ff128abe1f4a8b78e4b2 100644
--- a/source/armarx/navigation/core/Graph.h
+++ b/source/armarx/navigation/core/Graph.h
@@ -73,7 +73,7 @@ namespace armarx::navigation::core
return m_value;
}
- std::optional<core::Trajectory> trajectory = std::nullopt;
+ std::optional<core::GlobalTrajectory> trajectory = std::nullopt;
float m_value{0.F};
};
diff --git a/source/armarx/navigation/core/NavigatorInterface.h b/source/armarx/navigation/core/NavigatorInterface.h
index 05df688d1f161e4f0c39a2929cd65e6ebe73e1e9..83d5d460614d71db5de56137e3c2bfa4ac35c82b 100644
--- a/source/armarx/navigation/core/NavigatorInterface.h
+++ b/source/armarx/navigation/core/NavigatorInterface.h
@@ -6,6 +6,10 @@
namespace armarx::navigation::core
{
+ /**
+ * @brief Navigator interface for PointGoal navigation (with waypoints) and relative movement
+ *
+ */
class NavigatorInterface
{
@@ -19,6 +23,10 @@ namespace armarx::navigation::core
virtual void moveTo(const std::vector<client::WaypointTarget>& targets,
core::NavigationFrame navigationFrame) = 0;
+ virtual void moveToLocation(const std::string& location) = 0;
+
+ virtual void update(const std::vector<core::Pose>& waypoints,
+ core::NavigationFrame navigationFrame) = 0;
virtual void pause() = 0;
diff --git a/source/armarx/navigation/core/StaticScene.h b/source/armarx/navigation/core/StaticScene.h
index de0f008db6a521816bebb17d3534ba1903efc3f3..e55d7e6fd3969d8e9a1e72dbe839bd66391ccb96 100644
--- a/source/armarx/navigation/core/StaticScene.h
+++ b/source/armarx/navigation/core/StaticScene.h
@@ -23,9 +23,11 @@
#pragma once
#include <memory>
+#include <map>
#include <VirtualRobot/VirtualRobot.h>
+#include "armarx/navigation/core/basic_types.h"
#include <armarx/navigation/algorithms/Costmap.h>
namespace armarx::navigation::core
@@ -35,9 +37,9 @@ namespace armarx::navigation::core
{
VirtualRobot::SceneObjectSetPtr objects;
- // TODO(fabian.reister): rename, why unique_ptr
- std::unique_ptr<algorithms::Costmap> costmap;
+ std::optional<algorithms::Costmap> distanceToObstaclesCostmap;
+ std::map<std::string, core::Pose> locations;
};
diff --git a/source/armarx/navigation/core/Trajectory.cpp b/source/armarx/navigation/core/Trajectory.cpp
index a5bbc1ea2291983a2881004ed0f9c5414ee2041f..2265c6829b8b9c1913f57e51de9ec190f1e5f563 100644
--- a/source/armarx/navigation/core/Trajectory.cpp
+++ b/source/armarx/navigation/core/Trajectory.cpp
@@ -6,18 +6,6 @@
#include <iterator>
#include <limits>
-#include <range/v3/action/insert.hpp>
-#include <range/v3/numeric/accumulate.hpp>
-#include <range/v3/range/conversion.hpp>
-#include <range/v3/view/all.hpp>
-#include <range/v3/view/concat.hpp>
-#include <range/v3/view/for_each.hpp>
-#include <range/v3/view/group_by.hpp>
-#include <range/v3/view/reverse.hpp>
-#include <range/v3/view/sliding.hpp>
-#include <range/v3/view/transform.hpp>
-#include <range/v3/view/zip.hpp>
-
#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -34,6 +22,17 @@
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
+#include <range/v3/action/insert.hpp>
+#include <range/v3/numeric/accumulate.hpp>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/all.hpp>
+#include <range/v3/view/concat.hpp>
+#include <range/v3/view/for_each.hpp>
+#include <range/v3/view/group_by.hpp>
+#include <range/v3/view/reverse.hpp>
+#include <range/v3/view/sliding.hpp>
+#include <range/v3/view/transform.hpp>
+#include <range/v3/view/zip.hpp>
// FIXME move to simox
@@ -136,16 +135,16 @@ namespace armarx::navigation::core
namespace conv
{
- TrajectoryPoint
+ GlobalTrajectoryPoint
toTrajectoryPoint(const Pose& pose, const float velocity)
{
- return TrajectoryPoint{.waypoint = {.pose = pose}, .velocity = velocity};
+ return GlobalTrajectoryPoint{.waypoint = {.pose = pose}, .velocity = velocity};
}
- TrajectoryPoints
+ GlobalTrajectoryPoints
toTrajectoryPoints(const Path& path, const float velocity)
{
- TrajectoryPoints trajectoryPoints;
+ GlobalTrajectoryPoints trajectoryPoints;
trajectoryPoints.reserve(path.size());
std::transform(path.begin(),
@@ -158,13 +157,14 @@ namespace armarx::navigation::core
} // namespace conv
- Projection
- Trajectory::getProjection(const Position& point,
- const VelocityInterpolation& velocityInterpolation) const
+
+ GlobalTrajectory::InternalProjection
+ GlobalTrajectory::projectInternal(const Position& point,
+ const VelocityInterpolation& velocityInterpolation) const
{
float distance = std::numeric_limits<float>::max();
- Projection bestProj;
+ InternalProjection bestProj;
for (size_t i = 0; i < pts.size() - 1; i++)
{
@@ -198,26 +198,9 @@ namespace armarx::navigation::core
math::LinearInterpolatedPose ip(
wpBefore.waypoint.pose.matrix(), wpAfter.waypoint.pose.matrix(), 0, 1, true);
- bestProj.wayPointBefore = wpBefore;
- bestProj.wayPointAfter = wpAfter;
+ bestProj.indexBefore = i;
bestProj.projection.waypoint.pose = ip.Get(t);
- bestProj.segment = [&]
- {
- if (i == 0)
- {
- return Projection::Segment::FIRST;
- }
-
- if (i == (pts.size() - 2))
- {
- return Projection::Segment::FINAL;
- }
-
- return Projection::Segment::INTERMEDIATE;
- }();
-
-
switch (velocityInterpolation)
{
case VelocityInterpolation::LinearInterpolation:
@@ -236,10 +219,86 @@ namespace armarx::navigation::core
return bestProj;
}
+ Projection
+ GlobalTrajectory::getProjection(const Position& point,
+ const VelocityInterpolation& velocityInterpolation) const
+ {
+ InternalProjection intProj = projectInternal(point, velocityInterpolation);
+
+ const auto& wpBefore = pts.at(intProj.indexBefore);
+ const auto& wpAfter = pts.at(intProj.indexBefore + 1);
+
+ Projection bestProj;
+
+ bestProj.wayPointBefore = wpBefore;
+ bestProj.wayPointAfter = wpAfter;
+ bestProj.projection = intProj.projection;
+
+ bestProj.segment = [&]
+ {
+ if (intProj.indexBefore == 0)
+ {
+ return Projection::Segment::FIRST;
+ }
+
+ if (intProj.indexBefore == (pts.size() - 2))
+ {
+ return Projection::Segment::FINAL;
+ }
+
+ return Projection::Segment::INTERMEDIATE;
+ }();
+
+ return bestProj;
+ }
+
+ std::pair<GlobalTrajectory, bool>
+ GlobalTrajectory::getSubTrajectory(const Position& point, float distance) const
+ {
+ const InternalProjection intProj =
+ projectInternal(point, VelocityInterpolation::LinearInterpolation);
+
+ GlobalTrajectory subTraj{};
+ subTraj.mutablePoints().push_back(intProj.projection);
+
+ float remainingDistance = distance;
+ GlobalTrajectoryPoint lastWp = intProj.projection;
+ for (size_t i = intProj.indexBefore + 1; i < pts.size(); i++)
+ {
+ const auto& nextWp = pts.at(i);
+
+ const float segmentDistance =
+ (nextWp.waypoint.pose.translation() - lastWp.waypoint.pose.translation()).norm();
+
+ if (segmentDistance < remainingDistance)
+ {
+ subTraj.mutablePoints().push_back(nextWp);
+ lastWp = nextWp;
+ remainingDistance -= segmentDistance;
+ }
+ else
+ {
+ // fraction of distance between segment end points
+ const float t = remainingDistance / segmentDistance;
+
+ math::LinearInterpolatedPose ip(
+ lastWp.waypoint.pose.matrix(), nextWp.waypoint.pose.matrix(), 0, 1, true);
+ const float velocity = lastWp.velocity + (nextWp.velocity - lastWp.velocity) * t;
+
+ subTraj.mutablePoints().push_back({{static_cast<Pose>(ip.Get(t))}, velocity});
+
+ return {subTraj, false};
+ }
+ }
+
+ // remaining trajectory is shorter than the specified distance
+ return {subTraj, true};
+ }
+
std::vector<Eigen::Vector3f>
- Trajectory::positions() const noexcept
+ GlobalTrajectory::positions() const noexcept
{
- const auto toPosition = [](const TrajectoryPoint& wp)
+ const auto toPosition = [](const GlobalTrajectoryPoint& wp)
{ return wp.waypoint.pose.translation(); };
std::vector<Eigen::Vector3f> positions;
@@ -251,7 +310,7 @@ namespace armarx::navigation::core
}
std::vector<Pose>
- Trajectory::poses() const noexcept
+ GlobalTrajectory::poses() const noexcept
{
std::vector<Pose> poses;
poses.reserve(pts.size());
@@ -259,23 +318,23 @@ namespace armarx::navigation::core
std::transform(pts.begin(),
pts.end(),
std::back_inserter(poses),
- [](const core::TrajectoryPoint& pt) -> core::Pose
+ [](const core::GlobalTrajectoryPoint& pt) -> core::Pose
{ return pt.waypoint.pose; });
return poses;
}
- Trajectory
- Trajectory::FromPath(const Path& path, const float velocity)
+ GlobalTrajectory
+ GlobalTrajectory::FromPath(const Path& path, const float velocity)
{
return {conv::toTrajectoryPoints(path, velocity)};
}
- Trajectory
- Trajectory::FromPath(const Pose& start,
- const Positions& waypoints,
- const Pose& goal,
- const float velocity)
+ GlobalTrajectory
+ GlobalTrajectory::FromPath(const Pose& start,
+ const Positions& waypoints,
+ const Pose& goal,
+ const float velocity)
{
// currently, only 2D version
@@ -331,7 +390,8 @@ namespace armarx::navigation::core
{
core::Positions resampledPath;
- const auto toPoint = [](const TrajectoryPoint& wp) -> Pose { return wp.waypoint.pose; };
+ const auto toPoint = [](const GlobalTrajectoryPoint& wp) -> Pose
+ { return wp.waypoint.pose; };
const core::Path originalPath = pts | ranges::views::transform(toPoint) | ranges::to_vector;
@@ -424,8 +484,8 @@ namespace armarx::navigation::core
return resampledPath;
}
- Trajectory
- Trajectory::resample(const float eps) const
+ GlobalTrajectory
+ GlobalTrajectory::resample(const float eps) const
{
ARMARX_CHECK_GREATER_EQUAL(pts.size(), 2);
@@ -438,14 +498,14 @@ namespace armarx::navigation::core
ARMARX_DEBUG << "The resampled path contains no points. This means that it is likely "
"very short.";
- const core::Trajectory testTrajectory = core::Trajectory::FromPath(
+ const core::GlobalTrajectory testTrajectory = core::GlobalTrajectory::FromPath(
pts.front().waypoint.pose, resampledPathForward, pts.back().waypoint.pose, 0.F);
ARMARX_CHECK_LESS_EQUAL(testTrajectory.length(), eps)
<< "The resampled trajectory is only allowed to contains no points if it is "
"shorter than eps";
- return Trajectory({pts.front(), pts.back()});
+ return GlobalTrajectory({pts.front(), pts.back()});
}
@@ -488,12 +548,12 @@ namespace armarx::navigation::core
ranges::to_vector;
- auto resampledTrajectory = Trajectory::FromPath(
+ auto resampledTrajectory = GlobalTrajectory::FromPath(
pts.front().waypoint.pose, resampledPath, pts.back().waypoint.pose, 0.F);
// set velocity based on original trajectory
{
- const auto setVelocityInPlace = [&](TrajectoryPoint& pt)
+ const auto setVelocityInPlace = [&](GlobalTrajectoryPoint& pt)
{
const auto projection = getProjection(pt.waypoint.pose.translation(),
VelocityInterpolation::LinearInterpolation);
@@ -520,7 +580,7 @@ namespace armarx::navigation::core
}
float
- Trajectory::length() const
+ GlobalTrajectory::length() const
{
namespace r = ::ranges;
@@ -541,18 +601,18 @@ namespace armarx::navigation::core
}
void
- Trajectory::setMaxVelocity(float maxVelocity)
+ GlobalTrajectory::setMaxVelocity(float maxVelocity)
{
ARMARX_CHECK_GREATER_EQUAL(maxVelocity, 0.F) << "maxVelocity must be positive!";
std::for_each(pts.begin(),
pts.end(),
- [&maxVelocity](TrajectoryPoint& pt)
+ [&maxVelocity](GlobalTrajectoryPoint& pt)
{ pt.velocity = std::clamp(pt.velocity, -maxVelocity, maxVelocity); });
}
bool
- Trajectory::hasMaxDistanceBetweenWaypoints(const float maxDistance) const
+ GlobalTrajectory::hasMaxDistanceBetweenWaypoints(const float maxDistance) const
{
namespace r = ::ranges;
@@ -569,21 +629,21 @@ namespace armarx::navigation::core
rng, [&maxDistance](const auto& dist) -> bool { return dist > maxDistance; });
}
- const std::vector<TrajectoryPoint>&
- Trajectory::points() const
+ const std::vector<GlobalTrajectoryPoint>&
+ GlobalTrajectory::points() const
{
return pts;
}
- std::vector<TrajectoryPoint>&
- Trajectory::mutablePoints()
+ std::vector<GlobalTrajectoryPoint>&
+ GlobalTrajectory::mutablePoints()
{
return pts;
}
float
- Trajectory::duration(const core::VelocityInterpolation interpolation) const
+ GlobalTrajectory::duration(const core::VelocityInterpolation interpolation) const
{
float dur = 0;
@@ -601,7 +661,7 @@ namespace armarx::navigation::core
constexpr int nSamples = 100;
- for(int j = 0; j < nSamples; j++)
+ for (int j = 0; j < nSamples; j++)
{
const float progress = static_cast<float>(j) / nSamples;
@@ -618,16 +678,16 @@ namespace armarx::navigation::core
}
bool
- Trajectory::isValid() const noexcept
+ GlobalTrajectory::isValid() const noexcept
{
- const auto isValid = [](const TrajectoryPoint& pt) -> bool
+ const auto isValid = [](const GlobalTrajectoryPoint& pt) -> bool
{ return std::isfinite(pt.velocity) and pt.velocity < 3000; };
return std::all_of(pts.begin(), pts.end(), isValid);
}
- Trajectory::Indices
- Trajectory::allConnectedPointsInRange(std::size_t idx, float radius) const
+ GlobalTrajectory::Indices
+ GlobalTrajectory::allConnectedPointsInRange(std::size_t idx, float radius) const
{
const core::Position referencePosition = pts.at(idx).waypoint.pose.translation();
@@ -635,11 +695,11 @@ namespace armarx::navigation::core
{
const float posDiff =
(pts.at(i).waypoint.pose.translation() - referencePosition).norm();
- ARMARX_INFO << VAROUT(posDiff);
+ // ARMARX_INFO << VAROUT(posDiff);
return posDiff <= radius;
};
- Trajectory::Indices indices;
+ GlobalTrajectory::Indices indices;
// traverse from query point to start
@@ -670,10 +730,73 @@ namespace armarx::navigation::core
}
}
- ARMARX_INFO << indices.size() << " points in range";
+ ARMARX_DEBUG << indices.size() << " points in range";
return indices;
}
+ const std::vector<LocalTrajectoryPoint>&
+ LocalTrajectory::points() const
+ {
+ return pts;
+ }
+
+ std::vector<LocalTrajectoryPoint>&
+ LocalTrajectory::mutablePoints()
+ {
+ return pts;
+ }
+
+ Evaluation
+ LocalTrajectory::evaluate(const armarx::core::time::DateTime& timestamp) const
+ {
+ const auto cmp = [](const core::LocalTrajectoryPoint& lhs,
+ const DateTime& timestamp) -> bool
+ { return lhs.timestamp < timestamp; };
+
+ const auto lower = std::lower_bound(pts.begin(), pts.end(), timestamp, cmp);
+
+ if (lower == pts.end() - 1)
+ {
+ // if we arrived at the last point, the velocity is 0
+ return {lower->pose, core::Twist::Zero()};
+ }
+
+ const Duration d1 = timestamp - lower->timestamp;
+ const Duration dT = lower[1].timestamp - lower->timestamp;
+
+ // the waypoint before this timestamp
+ const auto& global_T_wp_before = lower->pose;
+
+ // the waypoint after this timestamp
+ const auto& global_T_wp_after = lower[1].pose;
+
+ // fraction of time between segment end points
+ const float t = d1 / dT;
+ math::LinearInterpolatedPose ip(
+ global_T_wp_before.matrix(), global_T_wp_after.matrix(), 0, 1, true);
+
+ const core::Pose wp_before_T_wp_after = global_T_wp_before.inverse() * global_T_wp_after;
+
+ const Eigen::Matrix3f& global_R_wp_before = global_T_wp_before.linear();
+
+ // the movement direction in the global frame
+ const Eigen::Vector3f global_V_linear_movement_direction =
+ global_R_wp_before * wp_before_T_wp_after.translation().normalized();
+ const Eigen::Vector3f distance = lower[1].pose.translation() - lower->pose.translation();
+ const float linearVelocity = static_cast<float>(distance.norm()) / dT.toSecondsDouble();
+
+
+ // angular velocity
+
+ Eigen::AngleAxisf angleAx(wp_before_T_wp_after.linear());
+
+ const core::Twist feedforwardTwist{
+ .linear = linearVelocity * global_V_linear_movement_direction,
+ .angular = angleAx.axis() * angleAx.angle() / dT.toSecondsDouble()};
+
+ return {static_cast<core::Pose>(ip.Get(t)), feedforwardTwist};
+ }
+
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/Trajectory.h b/source/armarx/navigation/core/Trajectory.h
index fd5f7e4f7d0def4e14d97ffbb6d69a09d047bf4d..20da0ba1f577865ab3e7938a1dddb4f3e565275a 100644
--- a/source/armarx/navigation/core/Trajectory.h
+++ b/source/armarx/navigation/core/Trajectory.h
@@ -25,25 +25,27 @@
#include <cstddef>
#include <memory>
+#include "ArmarXCore/core/time/DateTime.h"
+
#include <armarx/navigation/core/basic_types.h>
namespace armarx::navigation::core
{
- struct TrajectoryPoint
+ struct GlobalTrajectoryPoint
{
Waypoint waypoint;
float velocity; // [mm/s]
};
- using TrajectoryPoints = std::vector<TrajectoryPoint>;
+ using GlobalTrajectoryPoints = std::vector<GlobalTrajectoryPoint>;
struct Projection
{
- TrajectoryPoint projection;
+ GlobalTrajectoryPoint projection;
- TrajectoryPoint wayPointBefore;
- TrajectoryPoint wayPointAfter;
+ GlobalTrajectoryPoint wayPointBefore;
+ GlobalTrajectoryPoint wayPointAfter;
enum class Segment
{
@@ -61,20 +63,29 @@ namespace armarx::navigation::core
LastWaypoint
};
- using TrajectoryPtr = std::shared_ptr<class Trajectory>;
+ using GlobalTrajectoryPtr = std::shared_ptr<class GlobalTrajectory>;
- class Trajectory
+ class GlobalTrajectory
{
public:
- Trajectory() = default;
+ GlobalTrajectory() = default;
- Trajectory(const std::vector<TrajectoryPoint>& points) : pts(points)
+ GlobalTrajectory(const std::vector<GlobalTrajectoryPoint>& points) : pts(points)
{
}
Projection getProjection(const Position& point,
const VelocityInterpolation& velocityInterpolation) const;
+ /**
+ * @brief Project point onto the trajectory and return a new trajectory along the old one
+ * from that point for the specified distance.
+ * @return The subtrajectory and whether the subtrajectory ends at the same point,
+ * as this trajectory
+ */
+ std::pair<GlobalTrajectory, bool> getSubTrajectory(const Position& point,
+ const float distance) const;
+
[[nodiscard]] std::vector<Position> positions() const noexcept;
[[nodiscard]] std::vector<Pose> poses() const noexcept;
@@ -82,15 +93,15 @@ namespace armarx::navigation::core
//! Note: the velocity will not be set!
// currently, only 2D version
// y is pointing forward
- static Trajectory FromPath(const Path& path, float velocity);
+ static GlobalTrajectory FromPath(const Path& path, float velocity);
//! Note: the velocity will not be set!
// currently, only 2D version
// y is pointing forward
- static Trajectory
+ static GlobalTrajectory
FromPath(const Pose& start, const Positions& waypoints, const Pose& goal, float velocity);
- Trajectory resample(float eps) const;
+ GlobalTrajectory resample(float eps) const;
float length() const;
@@ -98,9 +109,9 @@ namespace armarx::navigation::core
bool hasMaxDistanceBetweenWaypoints(float maxDistance) const;
- const std::vector<TrajectoryPoint>& points() const;
+ const std::vector<GlobalTrajectoryPoint>& points() const;
- std::vector<TrajectoryPoint>& mutablePoints();
+ std::vector<GlobalTrajectoryPoint>& mutablePoints();
float duration(core::VelocityInterpolation interpolation) const;
@@ -116,9 +127,56 @@ namespace armarx::navigation::core
*/
Indices allConnectedPointsInRange(std::size_t idx, float radius) const;
+ private:
+ struct InternalProjection
+ {
+ GlobalTrajectoryPoint projection;
+ size_t indexBefore;
+ };
+ InternalProjection
+ projectInternal(const Position& point,
+ const VelocityInterpolation& velocityInterpolation) const;
+
private:
- std::vector<TrajectoryPoint> pts;
+ std::vector<GlobalTrajectoryPoint> pts;
+ };
+
+
+ struct LocalTrajectoryPoint
+ {
+ DateTime timestamp;
+ core::Pose pose;
+ };
+
+ using LocalTrajectoryPoints = std::vector<LocalTrajectoryPoint>;
+
+ struct Evaluation
+ {
+ core::Pose pose;
+ core::Twist feedforwardTwist; // [mm/s]
};
+
+ class LocalTrajectory
+ {
+ public:
+ LocalTrajectory() = default;
+
+ LocalTrajectory(const std::vector<LocalTrajectoryPoint>& points) : pts(points)
+ {
+ }
+
+ const std::vector<LocalTrajectoryPoint>& points() const;
+
+ std::vector<LocalTrajectoryPoint>& mutablePoints();
+
+ Evaluation evaluate(const DateTime& timestamp) const;
+
+
+ private:
+ std::vector<LocalTrajectoryPoint> pts;
+ };
+
+
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/aron/Events.xml b/source/armarx/navigation/core/aron/Events.xml
index a0be50ecbc393daf60798934b61cde23862f8835..20d53656d839c54901b1a2bee51b4bfd5e5e51c3 100644
--- a/source/armarx/navigation/core/aron/Events.xml
+++ b/source/armarx/navigation/core/aron/Events.xml
@@ -1,12 +1,5 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- <!-- <Include include="<armarx/navigation/core/aron/GlobalPlannerParams.aron.generated.h>" /> -->
- </CodeIncludes>
- <AronIncludes>
- <!-- <Include include="<armarx/navigation/global_planning/aron/GlobalPlannerParams.xml>" /> -->
- </AronIncludes>
-
<GenerateTypes>
<Object name='armarx::navigation::core::arondto::GoalReachedEvent'>
@@ -53,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/Graph.xml b/source/armarx/navigation/core/aron/Graph.xml
index 77fe69491bfafb9e3d0455fb0d0e72d4b03d8a5f..e4d07bc0851a131c0d31d28a631e7c085f4e28bb 100644
--- a/source/armarx/navigation/core/aron/Graph.xml
+++ b/source/armarx/navigation/core/aron/Graph.xml
@@ -1,10 +1,7 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- </CodeIncludes>
<AronIncludes>
- <!-- <Include include="<armarx/navigation/location/aron/Location.xml>" autoinclude="true" /> -->
- <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" />
</AronIncludes>
<GenerateTypes>
diff --git a/source/armarx/navigation/core/aron/Location.xml b/source/armarx/navigation/core/aron/Location.xml
index 9a0539d5d40a3b3b91dc32ad2034077a815437ff..246103c503a29dfcfd1a7de6aef7418da89c61e6 100644
--- a/source/armarx/navigation/core/aron/Location.xml
+++ b/source/armarx/navigation/core/aron/Location.xml
@@ -1,7 +1,5 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- </CodeIncludes>
<AronIncludes>
<Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true"/>
</AronIncludes>
diff --git a/source/armarx/navigation/core/aron/Trajectory.xml b/source/armarx/navigation/core/aron/Trajectory.xml
index 7e3c96c5b13c556e1e4d5e61fe886080618a9256..6981d471154d3b71b576c00d48a9fa92d57901d5 100644
--- a/source/armarx/navigation/core/aron/Trajectory.xml
+++ b/source/armarx/navigation/core/aron/Trajectory.xml
@@ -1,14 +1,8 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- <!-- <Include include="<armarx/navigation/core/aron/GlobalPlannerParams.aron.generated.h>" /> -->
- </CodeIncludes>
- <AronIncludes>
- <!-- <Include include="<armarx/navigation/global_planning/aron/GlobalPlannerParams.xml>" /> -->
- </AronIncludes>
-
<GenerateTypes>
- <Object name='armarx::navigation::core::arondto::TrajectoryPoint'>
+ <!-- Global trajectory -->
+ <Object name='armarx::navigation::core::arondto::GlobalTrajectoryPoint'>
<ObjectChild key='velocity'>
<float />
</ObjectChild>
@@ -17,10 +11,28 @@
</ObjectChild>
</Object>
- <Object name='armarx::navigation::core::arondto::Trajectory'>
+ <Object name='armarx::navigation::core::arondto::GlobalTrajectory'>
+ <ObjectChild key='points'>
+ <List>
+ <armarx::navigation::core::arondto::GlobalTrajectoryPoint />
+ </List>
+ </ObjectChild>
+ </Object>
+
+ <!-- Local trajectory -->
+ <Object name='armarx::navigation::core::arondto::LocalTrajectoryPoint'>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='timestamp'>
+ <Time />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::core::arondto::LocalTrajectory'>
<ObjectChild key='points'>
<List>
- <armarx::navigation::core::arondto::TrajectoryPoint />
+ <armarx::navigation::core::arondto::LocalTrajectoryPoint />
</List>
</ObjectChild>
</Object>
diff --git a/source/armarx/navigation/core/aron/Twist.xml b/source/armarx/navigation/core/aron/Twist.xml
index 4fc4b4d773b5738318192245dec1d67d404f5314..f5da361e38e4a7e475b96d5973a8927dc5e6e20e 100644
--- a/source/armarx/navigation/core/aron/Twist.xml
+++ b/source/armarx/navigation/core/aron/Twist.xml
@@ -1,12 +1,5 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- <!-- <Include include="<armarx/navigation/core/aron/GlobalPlannerParams.aron.generated.h>" /> -->
- </CodeIncludes>
- <AronIncludes>
- <!-- <Include include="<armarx/navigation/global_planning/aron/GlobalPlannerParams.xml>" /> -->
- </AronIncludes>
-
<GenerateTypes>
<Object name='armarx::navigation::core::arondto::Twist'>
<ObjectChild key='linear'>
diff --git a/source/armarx/navigation/core/aron/TwistLimits.xml b/source/armarx/navigation/core/aron/TwistLimits.xml
index 4c06b38ee47364336f465cbc8e4fd6401b457deb..e42ef38c98c042a6f66f0f511e576ecb92924610 100644
--- a/source/armarx/navigation/core/aron/TwistLimits.xml
+++ b/source/armarx/navigation/core/aron/TwistLimits.xml
@@ -1,12 +1,6 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- </CodeIncludes>
- <AronIncludes>
- </AronIncludes>
-
<GenerateTypes>
-
<Object name='armarx::navigation::core::arondto::TwistLimits'>
<ObjectChild key='linear'>
<float />
diff --git a/source/armarx/navigation/core/aron_conversions.cpp b/source/armarx/navigation/core/aron_conversions.cpp
index 7b93cdc279833f1cd8f2467cd6950c98a73a8c38..2a720332f8c60a6b4dd7f73d44243f7743acc0d8 100644
--- a/source/armarx/navigation/core/aron_conversions.cpp
+++ b/source/armarx/navigation/core/aron_conversions.cpp
@@ -1,42 +1,41 @@
#include "aron_conversions.h"
-#include <range/v3/range/conversion.hpp>
-#include <range/v3/view/transform.hpp>
-
-#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/core/Trajectory.h>
#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
{
void
- toAron(arondto::TrajectoryPoint& dto, const TrajectoryPoint& bo)
+ toAron(arondto::GlobalTrajectoryPoint& dto, const GlobalTrajectoryPoint& bo)
{
dto.pose = bo.waypoint.pose.matrix();
dto.velocity = bo.velocity;
}
void
- fromAron(const arondto::TrajectoryPoint& dto, TrajectoryPoint& bo)
+ fromAron(const arondto::GlobalTrajectoryPoint& dto, GlobalTrajectoryPoint& bo)
{
bo.waypoint.pose = core::Pose(dto.pose);
bo.velocity = dto.velocity;
}
void
- toAron(arondto::Trajectory& dto, const Trajectory& bo)
+ toAron(arondto::GlobalTrajectory& dto, const GlobalTrajectory& bo)
{
dto.points = bo.points() |
ranges::views::transform(
- [](const TrajectoryPoint& boTp) -> arondto::TrajectoryPoint
+ [](const GlobalTrajectoryPoint& boTp) -> arondto::GlobalTrajectoryPoint
{
- arondto::TrajectoryPoint dtoTp;
+ arondto::GlobalTrajectoryPoint dtoTp;
toAron(dtoTp, boTp);
return dtoTp;
}) |
@@ -44,21 +43,66 @@ namespace armarx::navigation::core
}
void
- fromAron(const arondto::Trajectory& dto, Trajectory& bo)
+ fromAron(const arondto::GlobalTrajectory& dto, GlobalTrajectory& bo)
{
- bo = Trajectory(
+ bo = GlobalTrajectory(
dto.points |
ranges::views::
- transform( //static_cast<TrajectoryPoint (*)(const arondto::TrajectoryPoint&)>(&fromAron)
- [](const arondto::TrajectoryPoint& dto) -> TrajectoryPoint
+ transform( //static_cast<TrajectoryPoint (*)(const arondto::GlobalTrajectoryPoint&)>(&fromAron)
+ [](const arondto::GlobalTrajectoryPoint& dto) -> GlobalTrajectoryPoint
{
- TrajectoryPoint bo;
+ GlobalTrajectoryPoint bo;
fromAron(dto, bo);
return bo;
}) |
ranges::to_vector);
}
+
+ void
+ toAron(arondto::LocalTrajectoryPoint& dto, const LocalTrajectoryPoint& bo)
+ {
+ dto.pose = bo.pose.matrix();
+ dto.timestamp = bo.timestamp;
+ }
+
+ void
+ fromAron(const arondto::LocalTrajectoryPoint& dto, LocalTrajectoryPoint& bo)
+ {
+ bo.pose = core::Pose(dto.pose);
+ bo.timestamp = dto.timestamp;
+ }
+
+ void
+ toAron(arondto::LocalTrajectory& dto, const LocalTrajectory& bo)
+ {
+ dto.points = bo.points() |
+ ranges::views::transform(
+ [](const LocalTrajectoryPoint& boTp) -> arondto::LocalTrajectoryPoint
+ {
+ arondto::LocalTrajectoryPoint dtoTp;
+ toAron(dtoTp, boTp);
+ return dtoTp;
+ }) |
+ ranges::to_vector;
+ }
+
+ void
+ fromAron(const arondto::LocalTrajectory& dto, LocalTrajectory& bo)
+ {
+ bo =
+ dto.points |
+ ranges::views::
+ transform( //static_cast<TrajectoryPoint (*)(const arondto::GlobalTrajectoryPoint&)>(&fromAron)
+ [](const arondto::LocalTrajectoryPoint& dto) -> LocalTrajectoryPoint
+ {
+ LocalTrajectoryPoint bo;
+ fromAron(dto, bo);
+ return bo;
+ }) |
+ ranges::to_vector;
+ }
+
void
toAron(arondto::Twist& dto, const Twist& bo)
{
@@ -227,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 50f6cb44c5ad62cdf76d14927010042206c30d4a..408385df56046ca7b82d9a6fe310bd8e0d0aa879 100644
--- a/source/armarx/navigation/core/aron_conversions.h
+++ b/source/armarx/navigation/core/aron_conversions.h
@@ -41,11 +41,17 @@ namespace armarx::navigation::core
return dto;
}
- void toAron(arondto::TrajectoryPoint& dto, const TrajectoryPoint& bo);
- void fromAron(const arondto::TrajectoryPoint& dto, TrajectoryPoint& bo);
+ void toAron(arondto::GlobalTrajectoryPoint& dto, const GlobalTrajectoryPoint& bo);
+ void fromAron(const arondto::GlobalTrajectoryPoint& dto, GlobalTrajectoryPoint& bo);
- void toAron(arondto::Trajectory& dto, const Trajectory& bo);
- void fromAron(const arondto::Trajectory& dto, Trajectory& bo);
+ void toAron(arondto::GlobalTrajectory& dto, const GlobalTrajectory& bo);
+ void fromAron(const arondto::GlobalTrajectory& dto, GlobalTrajectory& bo);
+
+ void toAron(arondto::LocalTrajectoryPoint& dto, const LocalTrajectoryPoint& bo);
+ void fromAron(const arondto::LocalTrajectoryPoint& dto, LocalTrajectoryPoint& bo);
+
+ void toAron(arondto::LocalTrajectory& dto, const LocalTrajectory& bo);
+ void fromAron(const arondto::LocalTrajectory& dto, LocalTrajectory& bo);
void toAron(arondto::Twist& dto, const Twist& bo);
void fromAron(const arondto::Twist& dto, Twist& bo);
@@ -76,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/basic_types.cpp b/source/armarx/navigation/core/basic_types.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..460c560bc8905f63a3b3eb168193c6faf5d11bef
--- /dev/null
+++ b/source/armarx/navigation/core/basic_types.cpp
@@ -0,0 +1,18 @@
+#include "basic_types.h"
+
+namespace armarx::navigation::core
+{
+ core::Pose
+ Twist::poseDiff(const float dt) const
+ {
+ return core::Pose(Eigen::Translation3f(linear * dt)) *
+ core::Pose(Eigen::AngleAxisf(angular.norm() * dt, angular.normalized()));
+ }
+
+ Twist
+ Twist::Zero()
+ {
+ static const core::Twist zero{Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()};
+ return zero;
+ }
+} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/basic_types.h b/source/armarx/navigation/core/basic_types.h
index 0917270df14fcf9568f56f05ddb4a7492cc9e70e..6121a739f2359abe352a61ff82d765cf21c7c306 100644
--- a/source/armarx/navigation/core/basic_types.h
+++ b/source/armarx/navigation/core/basic_types.h
@@ -50,4 +50,14 @@ namespace armarx::navigation::core
Pose pose;
};
+ struct Twist
+ {
+ LinearVelocity linear;
+ AngularVelocity angular;
+
+ Pose poseDiff(float dt) const;
+
+ static Twist Zero();
+ };
+
} // namespace armarx::navigation::core
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/core/test/coreTest.cpp b/source/armarx/navigation/core/test/coreTest.cpp
index c62597ce82a30d3e460fbdbdc72b30ee263f0709..a176e269fe5c505cc8d3f3e12b5480049f88b7b0 100644
--- a/source/armarx/navigation/core/test/coreTest.cpp
+++ b/source/armarx/navigation/core/test/coreTest.cpp
@@ -47,7 +47,7 @@ BOOST_AUTO_TEST_CASE(testPathLength)
armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0)),
armarx::navigation::core::Pose(Eigen::Translation3f(0, 4000, 0))};
- const auto traj = armarx::navigation::core::Trajectory::FromPath(path, 100);
+ const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
BOOST_CHECK_CLOSE(traj.length(), 4000, 0.01);
}
@@ -57,7 +57,7 @@ BOOST_AUTO_TEST_CASE(testResampleAlongLine)
armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0))};
- const auto traj = armarx::navigation::core::Trajectory::FromPath(path, 100);
+ const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
BOOST_CHECK_EQUAL(traj.points().size(), 2);
const auto resampledTraj = traj.resample(500);
@@ -77,7 +77,7 @@ BOOST_AUTO_TEST_CASE(testResampleAlongLineWithWaypoint)
armarx::navigation::core::Pose(Eigen::Translation3f(0, 1050, 0)),
armarx::navigation::core::Pose(Eigen::Translation3f(0, 2100, 0))};
- const auto traj = armarx::navigation::core::Trajectory::FromPath(path, 100);
+ const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
BOOST_CHECK_EQUAL(traj.points().size(), 3);
const auto resampledTraj = traj.resample(500);
diff --git a/source/armarx/navigation/core/types.cpp b/source/armarx/navigation/core/types.cpp
index 346cd4de2e7bc71ef3b0ec7ac7acc3f22a6e995c..7a25d9285728277b8121613da84fe939a7d3d4f1 100644
--- a/source/armarx/navigation/core/types.cpp
+++ b/source/armarx/navigation/core/types.cpp
@@ -2,17 +2,17 @@
namespace armarx::navigation::core
{
- core::Pose
- Twist::poseDiff(const float dt) const
- {
- return core::Pose(Eigen::Translation3f(linear * dt)) *
- core::Pose(Eigen::AngleAxisf(angular.norm() * dt, angular.normalized()));
- }
+ // core::Pose
+ // Twist::poseDiff(const float dt) const
+ // {
+ // return core::Pose(Eigen::Translation3f(linear * dt)) *
+ // core::Pose(Eigen::AngleAxisf(angular.norm() * dt, angular.normalized()));
+ // }
- Twist
- Twist::Zero()
- {
- static const core::Twist zero{Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()};
- return zero;
- }
+ // Twist
+ // Twist::Zero()
+ // {
+ // static const core::Twist zero{Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()};
+ // return zero;
+ // }
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/types.h b/source/armarx/navigation/core/types.h
index 7d338dfd201520aeef8a567d9427fc932503d09b..ade2630f76f818af1928cf77365246c79b923560 100644
--- a/source/armarx/navigation/core/types.h
+++ b/source/armarx/navigation/core/types.h
@@ -58,15 +58,7 @@ namespace armarx::navigation::core
};
- struct Twist
- {
- LinearVelocity linear;
- AngularVelocity angular;
-
- Pose poseDiff(float dt) const;
-
- static Twist Zero();
- };
+
class TimeServerInterface;
@@ -84,6 +76,7 @@ namespace armarx::navigation::core
std::optional<core::DynamicScene> dynamicScene = std::nullopt;
VirtualRobot::RobotPtr robot;
+ core::Twist platformVelocity = core::Twist::Zero();
std::optional<core::SceneGraph> graph;
};
diff --git a/source/armarx/navigation/dynamic_scene/CMakeLists.txt b/source/armarx/navigation/dynamic_scene/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5a8b485ab7855871fb54aa40c7ca79d6901b0f7b
--- /dev/null
+++ b/source/armarx/navigation/dynamic_scene/CMakeLists.txt
@@ -0,0 +1,15 @@
+
+armarx_add_library(dynamic_scene
+DEPENDENCIES_PUBLIC
+ ArmarXCoreInterfaces
+ ArmarXCore
+ armarx_navigation::core
+ armarx_navigation::conversions
+DEPENDENCIES_PRIVATE
+ range-v3::range-v3
+SOURCES
+ #LocalPlanner.cpp
+HEADERS
+ #LocalPlanner.h
+INTERFACE # TODO SALt: remove this line once you added some source files!
+)
diff --git a/source/armarx/navigation/factories/LocalPlannerFactory.cpp b/source/armarx/navigation/factories/LocalPlannerFactory.cpp
index aed7f192efabc4a752ae77d2a054e1737839688e..f619fd37551972b8c36f39bbdaceca4a5fe47e4b 100644
--- a/source/armarx/navigation/factories/LocalPlannerFactory.cpp
+++ b/source/armarx/navigation/factories/LocalPlannerFactory.cpp
@@ -5,15 +5,19 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
+#include <armarx/navigation/local_planning/core.h>
#include <armarx/navigation/core/constants.h>
+
+#ifdef TIMED_ELASTIC_BANDS_ENABLED
#include <armarx/navigation/local_planning/TimedElasticBands.h>
+#endif
namespace armarx::navigation::fac
{
- loc_plan::LocalPlannerPtr
+ local_planning::LocalPlannerPtr
LocalPlannerFactory::create(const aron::data::DictPtr& params, const core::Scene& ctx)
{
- namespace layer = loc_plan;
+ namespace layer = local_planning;
if (not params)
{
@@ -29,12 +33,15 @@ namespace armarx::navigation::fac
const auto algoParams = aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
ARMARX_CHECK_NOT_NULL(algoParams);
- loc_plan::LocalPlannerPtr localPlanner;
+ local_planning::LocalPlannerPtr localPlanner;
switch (algo)
{
- case loc_plan::Algorithms::TimedElasticBands:
- localPlanner = std::make_shared<loc_plan::TimedElasticBands>(
- loc_plan::TimedElasticBandsParams::FromAron(algoParams), ctx);
+ case local_planning::Algorithms::TimedElasticBands:
+ #ifdef TIMED_ELASTIC_BANDS_ENABLED
+ localPlanner = std::make_shared<local_planning::TimedElasticBands>(
+ local_planning::TimedElasticBandsParams::FromAron(algoParams), ctx);
+ #endif
+
break;
}
diff --git a/source/armarx/navigation/factories/LocalPlannerFactory.h b/source/armarx/navigation/factories/LocalPlannerFactory.h
index 4e3b0e09aa6d349e580a70b515cf7f06ab46bfb8..124e4c88ea43d47ed5e36c8bb90ffa2580bd7e54 100644
--- a/source/armarx/navigation/factories/LocalPlannerFactory.h
+++ b/source/armarx/navigation/factories/LocalPlannerFactory.h
@@ -31,13 +31,13 @@ namespace armarx::navigation::fac
{
/**
- * @brief Factory to create a loc_plan::LocalPlanner
+ * @brief Factory to create a local_planning::LocalPlanner
*
*/
class LocalPlannerFactory
{
public:
- static loc_plan::LocalPlannerPtr create(const aron::data::DictPtr& params,
+ static local_planning::LocalPlannerPtr create(const aron::data::DictPtr& params,
const core::Scene& ctx);
protected:
diff --git a/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp b/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
index 3b8f6a392c37c4a45dfcaf873564158c138c92a2..0e628b2792713311f8f4ca39736180b4775984fb 100644
--- a/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
+++ b/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
@@ -5,17 +5,16 @@
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
#include <armarx/navigation/core/constants.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
-#include <armarx/navigation/trajectory_control/WaypointController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
namespace armarx::navigation::fac
{
- traj_ctrl::TrajectoryControllerPtr
+ traj_ctrl::local::TrajectoryControllerPtr
TrajectoryControllerFactory::create(const VirtualRobot::RobotPtr& robot,
const aron::data::DictPtr& params)
{
- namespace layer = traj_ctrl;
+ namespace layer = traj_ctrl::local;
if (not params)
{
@@ -31,16 +30,12 @@ namespace armarx::navigation::fac
const auto algoParams = aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
ARMARX_CHECK_NOT_NULL(algoParams);
- traj_ctrl::TrajectoryControllerPtr controller;
+ traj_ctrl::local::TrajectoryControllerPtr controller;
switch (algo)
{
- case traj_ctrl::Algorithms::WaypointController:
- controller = std::make_shared<traj_ctrl::WaypointController>(
- traj_ctrl::WaypointControllerParams::FromAron(algoParams));
- break;
- case traj_ctrl::Algorithms::TrajectoryFollowingController:
- controller = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
- robot, traj_ctrl::TrajectoryFollowingControllerParams::FromAron(algoParams));
+ case traj_ctrl::local::Algorithms::TrajectoryFollowingController:
+ controller = std::make_shared<traj_ctrl::local::TrajectoryFollowingController>(
+ robot, traj_ctrl::local::TrajectoryFollowingControllerParams::FromAron(algoParams));
break;
}
diff --git a/source/armarx/navigation/factories/TrajectoryControllerFactory.h b/source/armarx/navigation/factories/TrajectoryControllerFactory.h
index cd35d86046979841b263ad183b87fefdf6ec9643..e2528c95319945e8e54f73c3e4c3b7685328794c 100644
--- a/source/armarx/navigation/factories/TrajectoryControllerFactory.h
+++ b/source/armarx/navigation/factories/TrajectoryControllerFactory.h
@@ -22,27 +22,26 @@
#pragma once
-// #include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <VirtualRobot/VirtualRobot.h>
+
#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
#include <armarx/navigation/core/fwd.h>
#include <armarx/navigation/factories/fwd.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::fac
{
/**
- * @brief Factory to create a traj_ctrl::TrajectoryController
- *
- */
+ * @brief Factory to create a traj_ctrl::TrajectoryController
+ *
+ */
class TrajectoryControllerFactory
{
public:
- static traj_ctrl::TrajectoryControllerPtr create(const VirtualRobot::RobotPtr& robot,
- const aron::data::DictPtr& params);
+ static traj_ctrl::local::TrajectoryControllerPtr create(const VirtualRobot::RobotPtr& robot,
+ const aron::data::DictPtr& params);
- protected:
- private:
};
} // namespace armarx::navigation::fac
diff --git a/source/armarx/navigation/factories/fwd.h b/source/armarx/navigation/factories/fwd.h
index 7975180e3e3d6000b1dc1d87ef4e19f23f6bc496..79d60ad1fa8c9cde96883fb51b0722f3222a6806 100644
--- a/source/armarx/navigation/factories/fwd.h
+++ b/source/armarx/navigation/factories/fwd.h
@@ -32,11 +32,11 @@ namespace armarx::navigation
using GlobalPlannerPtr = std::shared_ptr<GlobalPlanner>;
} // namespace global_planning
- namespace loc_plan
+ namespace local_planning
{
class LocalPlanner;
using LocalPlannerPtr = std::shared_ptr<LocalPlanner>;
- } // namespace loc_plan
+ } // namespace local_planning
namespace traj_ctrl
{
diff --git a/source/armarx/navigation/factories/test/factoriesTest.cpp b/source/armarx/navigation/factories/test/factoriesTest.cpp
index 587ca5051d0cb50eb37dd3549cca0771b46a8e69..e10ba383e4852f7bb36edbbe9f62ed614f6699c1 100644
--- a/source/armarx/navigation/factories/test/factoriesTest.cpp
+++ b/source/armarx/navigation/factories/test/factoriesTest.cpp
@@ -24,7 +24,7 @@
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/AStar.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::factories
#define ARMARX_BOOST_TEST
@@ -55,7 +55,7 @@ BOOST_AUTO_TEST_CASE(testStackCreation)
armarx::navigation::client::NavigationStackConfig cfg;
cfg.globalPlanner(armarx::navigation::global_planning::AStarParams())
.trajectoryController(
- armarx::navigation::traj_ctrl::TrajectoryFollowingController::Params());
+ armarx::navigation::traj_ctrl::local::TrajectoryFollowingController::Params());
const auto dto = cfg.toAron();
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index 3dcd307cdeb00462033d892afbeadebf1556b479..276f1f3acbf60f4ea7c44c7723fa624ee31d498a 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -159,7 +159,7 @@ namespace armarx::navigation::global_planning
ARMARX_TRACE;
// FIXME check if costmap is available
- algorithm::astar::AStarPlanner planner(*scene.staticScene->costmap);
+ algorithm::astar::AStarPlanner planner(*scene.staticScene->distanceToObstaclesCostmap);
const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
@@ -235,13 +235,13 @@ namespace armarx::navigation::global_planning
smoothPlan.pop_back();
ARMARX_TRACE;
- auto trajectory =
- core::Trajectory::FromPath(start, conv::to3D(smoothPlan), goal, params.linearVelocity);
+ auto trajectory = core::GlobalTrajectory::FromPath(
+ start, conv::to3D(smoothPlan), goal, params.linearVelocity);
// TODO(fabian.reister): resampling of trajectory
// FIXME param
- std::optional<core::Trajectory> resampledTrajectory;
+ std::optional<core::GlobalTrajectory> resampledTrajectory;
if (params.resampleDistance < 0)
{
@@ -262,8 +262,8 @@ namespace armarx::navigation::global_planning
}
}
- ARMARX_IMPORTANT << "Resampled trajectory contains " << resampledTrajectory->points().size()
- << " points";
+ ARMARX_VERBOSE << "Resampled trajectory contains " << resampledTrajectory->points().size()
+ << " points";
resampledTrajectory->setMaxVelocity(params.linearVelocity);
@@ -283,15 +283,16 @@ namespace armarx::navigation::global_planning
if (not result)
{
- ARMARX_ERROR << "Optimizer failure";
+ ARMARX_WARNING << "Optimizer failure";
return std::nullopt;
}
- // TODO circular path smoothing should be done now
+ ARMARX_TRACE;
- algorithm::CircularPathSmoothing smoothing;
- auto smoothTrajectory = smoothing.smooth(result.trajectory.value());
- smoothTrajectory.setMaxVelocity(params.linearVelocity);
+ // TODO circular path smoothing should be done now
+ // algorithm::CircularPathSmoothing smoothing;
+ // auto smoothTrajectory = smoothing.smooth(result.trajectory.value());
+ // smoothTrajectory.setMaxVelocity(params.linearVelocity);
ARMARX_TRACE;
return GlobalPlannerResult{.trajectory = result.trajectory.value()};
diff --git a/source/armarx/navigation/global_planning/GlobalPlanner.h b/source/armarx/navigation/global_planning/GlobalPlanner.h
index 1a028db1566ea96bf8c2a4f9ff060e9e9e8f0e9c..6af2c754ebf4823b938c1dbc86f56c0b6b355ced 100644
--- a/source/armarx/navigation/global_planning/GlobalPlanner.h
+++ b/source/armarx/navigation/global_planning/GlobalPlanner.h
@@ -37,7 +37,7 @@ namespace armarx::navigation::global_planning
struct GlobalPlannerResult
{
- core::Trajectory trajectory;
+ core::GlobalTrajectory trajectory;
};
diff --git a/source/armarx/navigation/global_planning/Point2Point.cpp b/source/armarx/navigation/global_planning/Point2Point.cpp
index e6714c9abc07b011b14d9834414153fdccb39c28..aa84b2d58a15d85bad8a0752bfc7ee28e0b3d8e2 100644
--- a/source/armarx/navigation/global_planning/Point2Point.cpp
+++ b/source/armarx/navigation/global_planning/Point2Point.cpp
@@ -60,16 +60,16 @@ namespace armarx::navigation::global_planning
std::optional<GlobalPlannerResult>
Point2Point::plan(const core::Pose& goal)
{
- std::vector<core::TrajectoryPoint> trajectory;
+ std::vector<core::GlobalTrajectoryPoint> trajectory;
if (params.includeStartPose)
{
- trajectory.push_back(core::TrajectoryPoint{
+ trajectory.push_back(core::GlobalTrajectoryPoint{
.waypoint = core::Waypoint{.pose = core::Pose(scene.robot->getGlobalPose())},
.velocity = params.velocity});
}
- trajectory.push_back(core::TrajectoryPoint{.waypoint = core::Waypoint{.pose = goal},
+ trajectory.push_back(core::GlobalTrajectoryPoint{.waypoint = core::Waypoint{.pose = goal},
.velocity = params.velocity});
return GlobalPlannerResult{.trajectory = trajectory};
@@ -78,15 +78,15 @@ namespace armarx::navigation::global_planning
std::optional<GlobalPlannerResult>
Point2Point::plan(const core::Pose& start, const core::Pose& goal)
{
- std::vector<core::TrajectoryPoint> trajectory;
+ std::vector<core::GlobalTrajectoryPoint> trajectory;
if (params.includeStartPose)
{
- trajectory.push_back(core::TrajectoryPoint{.waypoint = core::Waypoint{.pose = start},
+ trajectory.push_back(core::GlobalTrajectoryPoint{.waypoint = core::Waypoint{.pose = start},
.velocity = params.velocity});
}
- trajectory.push_back(core::TrajectoryPoint{.waypoint = core::Waypoint{.pose = goal},
+ trajectory.push_back(core::GlobalTrajectoryPoint{.waypoint = core::Waypoint{.pose = goal},
.velocity = params.velocity});
return GlobalPlannerResult{.trajectory = trajectory};
diff --git a/source/armarx/navigation/global_planning/SPFA.cpp b/source/armarx/navigation/global_planning/SPFA.cpp
index e5c1c7cd5e120126e86c16a3af3449f20419d476..17b1aaae7c201c19f878180b671e40bbf85bb3b0 100644
--- a/source/armarx/navigation/global_planning/SPFA.cpp
+++ b/source/armarx/navigation/global_planning/SPFA.cpp
@@ -91,8 +91,11 @@ namespace armarx::navigation::global_planning
// FIXME check if costmap is available
algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams;
- algorithms::spfa::ShortestPathFasterAlgorithm planner(*scene.staticScene->costmap,
- spfaParams);
+
+ ARMARX_CHECK(scene.staticScene.has_value());
+ ARMARX_CHECK(scene.staticScene->distanceToObstaclesCostmap.has_value());
+ algorithms::spfa::ShortestPathFasterAlgorithm planner(
+ *scene.staticScene->distanceToObstaclesCostmap, spfaParams);
const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
@@ -116,8 +119,8 @@ namespace armarx::navigation::global_planning
}
const auto timeEnd = IceUtil::Time::now();
- ARMARX_IMPORTANT << "A* planning took " << (timeEnd - timeStart).toMilliSeconds() << " ms";
- ARMARX_IMPORTANT << "Path contains " << plan.path.size() << " points";
+ ARMARX_VERBOSE << "A* planning took " << (timeEnd - timeStart).toMilliSeconds() << " ms";
+ ARMARX_VERBOSE << "Path contains " << plan.path.size() << " points";
if (plan.path.size() < 2) // failure
{
@@ -155,12 +158,13 @@ namespace armarx::navigation::global_planning
// smoothPlan.pop_back();
ARMARX_TRACE;
- auto trajectory = core::Trajectory::FromPath(start, wpts, goal, params.linearVelocity);
+ auto trajectory =
+ core::GlobalTrajectory::FromPath(start, wpts, goal, params.linearVelocity);
// TODO(fabian.reister): resampling of trajectory
// FIXME param
- std::optional<core::Trajectory> resampledTrajectory;
+ std::optional<core::GlobalTrajectory> resampledTrajectory;
try
{
@@ -180,8 +184,8 @@ namespace armarx::navigation::global_planning
resampledTrajectory = trajectory;
}
- ARMARX_IMPORTANT << "Resampled trajectory contains " << resampledTrajectory->points().size()
- << " points";
+ ARMARX_VERBOSE << "Resampled trajectory contains " << resampledTrajectory->points().size()
+ << " points";
resampledTrajectory->setMaxVelocity(params.linearVelocity);
@@ -211,14 +215,15 @@ namespace armarx::navigation::global_planning
// auto smoothTrajectory = smoothing.smooth(result.trajectory.value());
// smoothTrajectory.setMaxVelocity(params.linearVelocity);
- const auto costmap = scene.staticScene->costmap.get();
+ ARMARX_CHECK(scene.staticScene->distanceToObstaclesCostmap.has_value());
+ const auto& costmap = scene.staticScene->distanceToObstaclesCostmap.value();
for (auto& point : result.trajectory->mutablePoints())
{
const float distance = std::min<float>(
spfaParams.obstacleMaxDistance,
- costmap->value(Eigen::Vector2f{point.waypoint.pose.translation().head<2>()})
+ costmap.value(Eigen::Vector2f{point.waypoint.pose.translation().head<2>()})
.value_or(0.F));
if (spfaParams.obstacleDistanceCosts)
diff --git a/source/armarx/navigation/global_planning/SPFA.h b/source/armarx/navigation/global_planning/SPFA.h
index c0900376727e7d1ab8bfb192669e903b15c5f598..7ac298933175e4fd4a775a5631448dda4d126db1 100644
--- a/source/armarx/navigation/global_planning/SPFA.h
+++ b/source/armarx/navigation/global_planning/SPFA.h
@@ -24,6 +24,8 @@
#include <VirtualRobot/MathTools.h>
#include "GlobalPlanner.h"
+#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
+#include <armarx/navigation/global_planning/core.h>
#include <armarx/navigation/core/types.h>
namespace armarx::navigation::global_planning
@@ -45,6 +47,7 @@ namespace armarx::navigation::global_planning
float resampleDistance{-1};
+ algorithms::spfa::ShortestPathFasterAlgorithm::Parameters algo;
Algorithms algorithm() const override;
diff --git a/source/armarx/navigation/global_planning/aron/GlobalPlannerParams.xml b/source/armarx/navigation/global_planning/aron/GlobalPlannerParams.xml
index 8139d1129215fa1d566b51a2b9f9537d7bc18574..7271c7b638a1bd2a6d140e2c58dac06bb80d9fa3 100644
--- a/source/armarx/navigation/global_planning/aron/GlobalPlannerParams.xml
+++ b/source/armarx/navigation/global_planning/aron/GlobalPlannerParams.xml
@@ -1,10 +1,5 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- </CodeIncludes>
- <AronIncludes>
- </AronIncludes>
-
<GenerateTypes>
<Object name='armarx::navigation::global_planning::arondto::GlobalPlannerParams'>
diff --git a/source/armarx/navigation/global_planning/aron/SPFAParams.xml b/source/armarx/navigation/global_planning/aron/SPFAParams.xml
index 63b61b6f9b54346494a9c58db5e8e15b7805f82d..c1b2869ffa7a8b3884de5d37dd782a132ebd7c3c 100644
--- a/source/armarx/navigation/global_planning/aron/SPFAParams.xml
+++ b/source/armarx/navigation/global_planning/aron/SPFAParams.xml
@@ -2,6 +2,7 @@
<AronTypeDefinition>
<AronIncludes>
<Include include="armarx/navigation/global_planning/aron/GlobalPlannerParams.xml" />
+ <Include include="armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.xml" />
</AronIncludes>
<GenerateTypes>
@@ -15,6 +16,10 @@
<float />
</ObjectChild>
+ <ObjectChild key='algo'>
+ <armarx::navigation::algorithms::arondto::ShortestPathFasterAlgorithmParams/>
+ </ObjectChild>
+
</Object>
diff --git a/source/armarx/navigation/global_planning/aron_conversions.cpp b/source/armarx/navigation/global_planning/aron_conversions.cpp
index 358efefcb01c7c722d9f278bbea662e01dd7d99e..ef57566f2844cf8c3cee3a4e441e6dedde20506d 100644
--- a/source/armarx/navigation/global_planning/aron_conversions.cpp
+++ b/source/armarx/navigation/global_planning/aron_conversions.cpp
@@ -4,6 +4,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/global_planning/Point2Point.h>
@@ -69,6 +70,7 @@ namespace armarx::navigation::global_planning::aron_conv
aron::toAron(dto.linearVelocity, bo.linearVelocity);
aron::toAron(dto.resampleDistance, bo.resampleDistance);
+ algorithms::toAron(dto.algo, bo.algo);
}
void
@@ -79,6 +81,8 @@ namespace armarx::navigation::global_planning::aron_conv
aron::fromAron(dto.linearVelocity, bo.linearVelocity);
aron::fromAron(dto.resampleDistance, bo.resampleDistance);
+
+ algorithms::fromAron(dto.algo, bo.algo);
}
} // namespace armarx::navigation::global_planning::aron_conv
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
index 2a7b5712403afee53a54d84738970e0c1be7a008..206bc90ba6d956c9cf0b405ff2ba1fd679406671 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>
@@ -31,7 +29,7 @@
namespace armarx::navigation::global_planning::optimization
{
- OrientationOptimizer::OrientationOptimizer(const core::Trajectory& trajectory,
+ OrientationOptimizer::OrientationOptimizer(const core::GlobalTrajectory& trajectory,
const Params& params) :
trajectory(trajectory), params(params)
{
@@ -45,7 +43,7 @@ namespace armarx::navigation::global_planning::optimization
namespace r = ::ranges;
namespace rv = ::ranges::views;
- const auto toYaw = [](const core::TrajectoryPoint& pt) -> double
+ const auto toYaw = [](const core::GlobalTrajectoryPoint& pt) -> double
{ return simox::math::mat4f_to_xyyaw(pt.waypoint.pose.matrix()).z(); };
std::vector<double> orientations =
@@ -74,7 +72,7 @@ namespace armarx::navigation::global_planning::optimization
orientations.at(i) = std::atan2(vCombined.y(), vCombined.x());
}
- ARMARX_INFO << "orientations before: " << orientations;
+ ARMARX_VERBOSE << "orientations before: " << orientations;
// const float yawStart = orientations.front();
// const float yawEnd = orientations.back();
@@ -85,7 +83,7 @@ namespace armarx::navigation::global_planning::optimization
ceres::Problem problem;
- ARMARX_INFO << orientations.size() - 2 << " orientations to optimize";
+ ARMARX_VERBOSE << orientations.size() - 2 << " orientations to optimize";
// TODO https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_2d/pose_graph_2d.cc
// ceres::LocalParameterization* angle_local_parameterization =
@@ -194,14 +192,14 @@ namespace armarx::navigation::global_planning::optimization
// Run the solver!
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
- options.minimizer_progress_to_stdout = true;
+ options.minimizer_progress_to_stdout = false;
options.max_num_iterations = 100;
options.function_tolerance = 0.01;
ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
- std::cout << summary.FullReport() << "\n";
+ ARMARX_VERBOSE << summary.FullReport() << "\n";
// std::cout << summary.BriefReport() << "\n";
const auto clampInPlace = [](auto& val)
@@ -209,8 +207,8 @@ namespace armarx::navigation::global_planning::optimization
std::for_each(orientations.begin(), orientations.end(), clampInPlace);
- ARMARX_INFO << "orientations after: " << orientations;
- ARMARX_INFO << "Optimization: " << summary.iterations.size() << " iterations";
+ ARMARX_VERBOSE << "orientations after: " << orientations;
+ ARMARX_VERBOSE << "Optimization: " << summary.iterations.size() << " iterations";
if (not summary.IsSolutionUsable())
{
@@ -218,9 +216,9 @@ namespace armarx::navigation::global_planning::optimization
// TODO write to file
}
- const auto applyOrientation = [](const auto& p) -> core::TrajectoryPoint
+ const auto applyOrientation = [](const auto& p) -> core::GlobalTrajectoryPoint
{
- core::TrajectoryPoint tp = p.first;
+ core::GlobalTrajectoryPoint tp = p.first;
const float yaw = p.second;
tp.waypoint.pose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, yaw);
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h
index 62ef5548c0976625440e06a3ee0ad1c05cca2198..c99570914413ca224fc09fd629bb98ea1697b85e 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h
@@ -30,7 +30,7 @@ namespace armarx::navigation::global_planning::optimization
struct OrientationOptimizerParams
{
- float priorWeight{0.25F};
+ float priorWeight{0.5F};
float smoothnessWeight{0.4F};
float smoothnessWeightStartGoal{1.F};
@@ -45,12 +45,12 @@ namespace armarx::navigation::global_planning::optimization
public:
using Params = OrientationOptimizerParams;
- OrientationOptimizer(const core::Trajectory& trajectory, const Params& params);
+ OrientationOptimizer(const core::GlobalTrajectory& trajectory, const Params& params);
struct OptimizationResult
{
- std::optional<core::Trajectory> trajectory;
+ std::optional<core::GlobalTrajectory> trajectory;
operator bool() const noexcept
{
@@ -63,7 +63,7 @@ namespace armarx::navigation::global_planning::optimization
protected:
private:
- core::Trajectory trajectory;
+ core::GlobalTrajectory trajectory;
const Params params;
};
diff --git a/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp b/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp
index c562b0a7b3ba678e89052fd729ce5614595bc29d..6b3f59e3865755b4cbc59cffaf99ce7ce151fbfa 100644
--- a/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp
+++ b/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp
@@ -52,7 +52,7 @@ BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoalMinimalPrior)
path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 2000} *
Eigen::AngleAxis(M_PI_2f32, Eigen::Vector3f::UnitZ()));
- const auto trajectory = nav::core::Trajectory::FromPath(path, 100);
+ const auto trajectory = nav::core::GlobalTrajectory::FromPath(path, 100);
gp::OrientationOptimizer::Params params{
.priorWeight = 1.F, .smoothnessWeight = 0.F, .smoothnessWeightStartGoal = 0.0F};
@@ -78,7 +78,7 @@ BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoalMinimalSmoothness)
path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 2000} *
Eigen::AngleAxis(M_PI_2f32, Eigen::Vector3f::UnitZ()));
- const auto trajectory = nav::core::Trajectory::FromPath(path, 100);
+ const auto trajectory = nav::core::GlobalTrajectory::FromPath(path, 100);
gp::OrientationOptimizer::Params params{
.priorWeight = 0.F, .smoothnessWeight = 1.F, .smoothnessWeightStartGoal = 0.0F};
@@ -104,7 +104,7 @@ BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoalMinimalSmoothnessStar
path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 2000} *
Eigen::AngleAxis(M_PI_2f32, Eigen::Vector3f::UnitZ()));
- const auto trajectory = nav::core::Trajectory::FromPath(path, 100);
+ const auto trajectory = nav::core::GlobalTrajectory::FromPath(path, 100);
gp::OrientationOptimizer::Params params{
.priorWeight = 0.F, .smoothnessWeight = 0.F, .smoothnessWeightStartGoal = 1.0F};
@@ -130,7 +130,7 @@ BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoal)
path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 2000} *
Eigen::AngleAxis(M_PI_2f32, Eigen::Vector3f::UnitZ()));
- const auto trajectory = nav::core::Trajectory::FromPath(path, 100);
+ const auto trajectory = nav::core::GlobalTrajectory::FromPath(path, 100);
gp::OrientationOptimizer::Params params{};
@@ -176,7 +176,7 @@ BOOST_AUTO_TEST_CASE(testOrientationOptimizationCircular)
path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 500 * 20});
- const auto trajectory = nav::core::Trajectory::FromPath(path, 100);
+ const auto trajectory = nav::core::GlobalTrajectory::FromPath(path, 100);
// Here we don't care about the orientatation prior.
// The orientation of the intermediate points should converge to 0 (same direction as start and goal)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp
index b59ac4cf2f3cb846c95cb027f16cc796368ba376..c0f9df03a2f9168a91dfacba3befa60596a09d6f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp
@@ -41,13 +41,12 @@
#include "widgets/graph_scene/Scene.h"
#include "widgets/graph_scene/Widget.h"
#include "widgets/utils.h"
-#include <armarx/navigation/components/NavigationMemory/ComponentInterface.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
#include <armarx/navigation/location/constants.h>
-#include <armarx/navigation/components/NavigationMemory/ComponentInterface.h>
+#include <armarx/navigation/components/navigation_memory/ComponentInterface.h>
// Qt headers
#include <QDialog>
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..559fec1c1361dd08eb59cb25ead47ea32ba8fa2c
--- /dev/null
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -0,0 +1,108 @@
+# TODO remove teb_ once the ArmarX CMake migration is complete.
+# Note: "human" is a library defined in VisionX
+
+armarx_add_aron_library(teb_human_aron
+ ARON_FILES
+ aron/Human.xml
+)
+
+armarx_add_library(teb_human
+ DEPENDENCIES_PUBLIC
+ ArmarXCore
+ armarx_navigation::core
+ armarx_navigation::conversions
+ armem_human #VisionX
+ ukfm # RobotAPI
+ DEPENDENCIES_PRIVATE
+ range-v3::range-v3
+ SOURCES
+ types.cpp
+ aron_conversions.cpp
+ ProxemicZoneCreator.cpp
+ HumanSystemModel.cpp
+ UnscentedKalmanFilter_impl.cpp
+ HumanTracker.cpp
+ HumanFilter.cpp
+ HEADERS
+ types.h
+ aron_conversions.h
+ shapes.h
+ ProxemicZoneCreator.h
+ HumanSystemModel.h
+ HumanTracker.h
+ HumanFilter.h
+)
+
+option(SCIPLOT_TESTS "" OFF)
+
+if(${SCIPLOT_TESTS})
+
+ find_package(sciplot)
+
+ armarx_add_test(so2_kalman_test
+ TEST_FILES
+ test/so2kalmanFilterTest.cpp
+ DEPENDENCIES
+ ArmarXCore
+ armarx_navigation::teb_human
+ sciplot::sciplot
+ )
+
+ armarx_add_test(se2_kalman_test
+ TEST_FILES
+ test/se2KalmanFilterTest.cpp
+ DEPENDENCIES
+ ArmarXCore
+ armarx_navigation::teb_human
+ sciplot::sciplot
+ )
+
+ armarx_add_test(se2xV_kalman_test
+ TEST_FILES
+ test/se2xVkalmanFilterTest.cpp
+ DEPENDENCIES
+ ArmarXCore
+ armarx_navigation::teb_human
+ sciplot::sciplot
+ )
+
+ armarx_add_test(manif_kalman_test
+ TEST_FILES
+ test/manifKalmanTest.cpp
+ DEPENDENCIES
+ ArmarXCore
+ armarx_navigation::teb_human
+ sciplot::sciplot
+ )
+
+
+ armarx_add_test(se3_kalman_test
+ TEST_FILES
+ test/UnscentedKalmanFilterTest.cpp
+ DEPENDENCIES
+ ArmarXCore
+ ukfm
+ sciplot::sciplot
+ )
+
+endif()
+
+armarx_add_test(kalman_filter_test
+ TEST_FILES
+ test/kalman_filter_test.cpp
+ DEPENDENCIES
+ ArmarXCore
+ armarx_navigation::core
+ armarx_navigation::teb_human
+ range-v3::range-v3
+)
+
+armarx_add_test(human_tracker_test
+ TEST_FILES
+ test/human_tracker_test.cpp
+ DEPENDENCIES
+ ArmarXCore
+ armarx_navigation::core
+ armarx_navigation::teb_human
+ range-v3::range-v3
+)
diff --git a/source/armarx/navigation/human/HumanFilter.cpp b/source/armarx/navigation/human/HumanFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..baf14d078c65a1db0efb792bca330361d549cb52
--- /dev/null
+++ b/source/armarx/navigation/human/HumanFilter.cpp
@@ -0,0 +1,131 @@
+#include "HumanFilter.h"
+
+namespace armarx::navigation::human
+{
+
+ HumanFilter::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime)
+ {
+ //initialize new human for detected human
+ human.pose = pose0;
+ human.linearVelocity = Eigen::Vector2f::Zero();
+ human.detectionTime = detectionTime;
+ lastDetectedHuman = human;
+
+ //initialize new kalman filter for new detected human
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<2, 2>(0, 0) *= params.control_pos_cov * params.control_pos_cov;
+ Q.block<1, 1>(2, 2) *= params.control_rot_cov * params.control_rot_cov;
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<2, 2>(0, 0) *= params.obs_pos_cov * params.obs_pos_cov;
+ R.block<1, 1>(2, 2) *= params.obs_rot_cov * params.obs_rot_cov;
+
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<2, 2>(0, 0) *= params.initial_state_pos_cov * params.initial_state_pos_cov;
+ P0.block<1, 1>(2, 2) *= params.initial_state_rot_cov * params.initial_state_rot_cov;
+
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
+ UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * params.alpha;
+
+ //initial position and orientation according to detected human
+ SystemModelT::StateT state0 = toUkfState(pose0);
+
+ ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0);
+ }
+
+ void
+ HumanFilter::propagation(const armarx::core::time::DateTime& detectionTime)
+ {
+ double dt = (detectionTime - human.detectionTime).toSecondsDouble();
+
+ SystemModelT::ControlT control = toUkfControl(human.linearVelocity);
+ ukf->propagation(control, dt);
+
+ human.pose = fromUkfState(ukf->getCurrentState());
+ human.detectionTime = detectionTime;
+ }
+
+ void
+ HumanFilter::update(const core::Pose2D& pose, const DateTime& detectionTime)
+ {
+ double dt = (detectionTime - lastDetectedHuman.detectionTime).toSecondsDouble();
+
+ //update ukf with new observation
+ SystemModelT::ObsT observation = toUkfObs(pose);
+ ukf->update(observation);
+
+ core::Pose2D newPose = fromUkfState(ukf->getCurrentState());
+ newPose.linear() = pose.linear();
+
+ // calculate velocity
+ Eigen::Vector2f ds = newPose.translation() - lastDetectedHuman.pose.translation();
+ Eigen::Vector2f linVelocity = ds / dt;
+ // apply exponential smoothing to velocity
+ //TODO try other approaches?
+ Eigen::Vector2f newVelocity =
+ params.velocityAlpha * linVelocity + (1 - params.velocityAlpha) * human.linearVelocity;
+
+ //update stored information about the human
+ human.detectionTime = detectionTime;
+ human.pose = newPose;
+ human.linearVelocity = newVelocity;
+
+ lastDetectedHuman = human;
+ }
+
+ const Human&
+ HumanFilter::get() const
+ {
+ return human;
+ }
+
+ Duration
+ HumanFilter::detectionAge(const DateTime& currentTime) const
+ {
+ return (currentTime - lastDetectedHuman.detectionTime);
+ }
+
+
+ HumanFilter::SystemModelT::StateT
+ HumanFilter::toUkfState(const core::Pose2D& pose)
+ {
+ // [mm] to [m]
+ return {{pose.translation().x() / 1000,
+ pose.translation().y() / 1000,
+ 0 /*Eigen::Rotation2Df(pose.linear()).angle()*/}};
+ }
+
+ core::Pose2D
+ HumanFilter::fromUkfState(const SystemModelT::StateT& pose)
+ {
+ core::Pose2D pose2d = core::Pose2D::Identity();
+ // [m] to [mm]
+ pose2d.translation().x() = pose.pose.x() * 1000;
+ pose2d.translation().y() = pose.pose.y() * 1000;
+ pose2d.linear() = Eigen::Rotation2Df(0 /*pose.pose.angle()*/).toRotationMatrix();
+
+ return pose2d;
+ }
+
+ HumanFilter::SystemModelT::ControlT
+ HumanFilter::toUkfControl(const Eigen::Vector2f& vel)
+ {
+ // convert velocity from global frame to human frame (required by kalman filter)
+ const auto& global_R_human = human.pose.linear();
+ const auto& human_V_vel = global_R_human.inverse() * vel;
+
+ // [mm] to [m]
+ return {{human_V_vel.x() / 1000, human_V_vel.y() / 1000, 0}};
+ }
+
+ HumanFilter::SystemModelT::ObsT
+ HumanFilter::toUkfObs(const core::Pose2D& pose)
+ {
+ return SystemModelT::observationFunction(toUkfState(pose));
+ }
+
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanFilter.h b/source/armarx/navigation/human/HumanFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..aaed6eecb901335156aa6ee281b87183d4acf21f
--- /dev/null
+++ b/source/armarx/navigation/human/HumanFilter.h
@@ -0,0 +1,110 @@
+/**
+ * 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 <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::human
+{
+
+ /**
+ * @brief The HumanFilter class can be used to track and filter the state of a single human. It
+ * hides implementation detail on how the filtering is done. New information about the human can
+ * be fed by using the update method. The human itself can be obtained using the get method.
+ */
+ class HumanFilter
+ {
+
+ public:
+ struct Parameters
+ {
+ //TODO which values are appropriate?
+ double control_pos_cov = 0.01;
+ double control_rot_cov = 0.01;
+ double obs_pos_cov = 0.01;
+ double obs_rot_cov = 0.01;
+ double initial_state_pos_cov = 0.01;
+ double initial_state_rot_cov = 0.01;
+ double alpha = 0.5;
+
+ float velocityAlpha = 0.7;
+ };
+
+ /**
+ * @brief HumanFilter::HumanFilter creates a new filter that filters 2D poses (position and
+ * rotation in a two dimensional plane)
+ * @param pose0 the first known pose of the human
+ * @param detectionTime the point of detection
+ */
+ HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime);
+
+ /**
+ * @brief HumanFilter::propagation propagate the system model to the given point in time. This
+ * means that the human pose is updated according to the filters prediction for the given point
+ * in time. Should be called at most once between every two calls of HumanFilter::update.
+ * @param detectionTime the point in time for which the pose should be predicted
+ */
+ void propagation(const DateTime& detectionTime);
+ /**
+ * @brief HumanFilter::update update the filter a new detected pose of the tracked human and
+ * the detection time
+ * @param pose the new detected pose of the human
+ * @param detectionTime the detection time
+ */
+ void update(const core::Pose2D& pose, const DateTime& detectionTime);
+ /**
+ * @brief HumanFilter::get returns the human whose pose is filtered containing the most recent
+ * state
+ * @return the human
+ */
+ const Human& get() const;
+
+ Duration detectionAge(const DateTime& currentTime) const;
+
+ private:
+ using SystemModelT = kalman_filter::SystemModelSE2<double>;
+
+ SystemModelT::StateT toUkfState(const core::Pose2D& pose);
+ core::Pose2D fromUkfState(const SystemModelT::StateT& pose);
+ SystemModelT::ControlT toUkfControl(const Eigen::Vector2f& vel);
+ SystemModelT::ObsT toUkfObs(const core::Pose2D& pose);
+
+ private:
+ Parameters params{};
+ /**
+ * @brief oldHuman stores information about the human at the point in time of the last
+ * HumanFilter::update call
+ */
+ Human lastDetectedHuman{};
+ /**
+ * @brief human stores information about the human at the point in time of the last
+ * HumanFilter::propagation call
+ */
+ Human human{};
+ std::unique_ptr<UnscentedKalmanFilter<SystemModelT>> ukf = nullptr;
+ };
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanSystemModel.cpp b/source/armarx/navigation/human/HumanSystemModel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..80f6820535832028682dad6f14dc28c0723c0cb4
--- /dev/null
+++ b/source/armarx/navigation/human/HumanSystemModel.cpp
@@ -0,0 +1,171 @@
+#include "HumanSystemModel.h"
+
+namespace armarx::navigation::human::kalman_filter
+{
+
+ // ------------------------------ SO2xR2 -----------------------------------------
+
+ template <typename floatT>
+ typename SystemModelSO2xR2<floatT>::ObsT
+ SystemModelSO2xR2<floatT>::observationFunction(const SystemModelSO2xR2::StateT& state)
+ {
+ ObsT observation;
+ observation.segment(0, 1) = state.orientation.log().coeffs();
+ observation.segment(1, 2) = state.position;
+ return observation;
+ }
+
+ template <typename floatT>
+ typename SystemModelSO2xR2<floatT>::StateT
+ SystemModelSO2xR2<floatT>::propagationFunction(const SystemModelSO2xR2::StateT& state,
+ const SystemModelSO2xR2::ControlT& control,
+ const SystemModelSO2xR2::ControlNoiseT& noise,
+ FloatT dt)
+ {
+ StateT new_state;
+ new_state.orientation = state.orientation.template rplus(control.angular_velocity * dt);
+ new_state.position = state.position + control.euclidean_velocity * dt;
+ //new_state.position =
+ // state.position + state.orientation.rotation() * control.euclidean_velocity * dt;
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSO2xR2<floatT>::StateT
+ SystemModelSO2xR2<floatT>::retraction(const SystemModelSO2xR2::StateT& state,
+ const SystemModelSO2xR2::SigmaPointsT& sigmaPoints)
+ {
+ StateT new_state;
+ manif::SO2Tangent<FloatT> tan;
+ tan.coeffs() << sigmaPoints.segment(0, 1);
+ //new_state.orientation = state.orientation.lplus(tan);
+ new_state.orientation = state.orientation.rplus(tan);
+
+ new_state.position = state.position + sigmaPoints.segment(1, 2);
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSO2xR2<floatT>::SigmaPointsT
+ SystemModelSO2xR2<floatT>::inverseRetraction(const SystemModelSO2xR2::StateT& state1,
+ const SystemModelSO2xR2::StateT& state2)
+ {
+ SigmaPointsT sigma;
+ // TODO: check if right order of substraction
+
+ //sigma.segment(0, 1) = state2.orientation.lminus(state1.orientation).coeffs();
+ sigma.segment(0, 1) = state1.orientation.lminus(state2.orientation).coeffs();
+
+ sigma.segment(1, 2) = state1.position - state2.position;
+ //sigma.segment(1, 2) = state2.position - state1.position;
+
+ return sigma;
+ }
+
+ template struct SystemModelSO2xR2<float>;
+ template struct SystemModelSO2xR2<double>;
+
+
+ // ------------------------------ SE2 -----------------------------------------
+
+ template <typename floatT>
+ typename SystemModelSE2<floatT>::ObsT
+ SystemModelSE2<floatT>::observationFunction(const SystemModelSE2::StateT& state)
+ {
+ ObsT observation = state.pose.log().coeffs();
+ return observation;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2<floatT>::StateT
+ SystemModelSE2<floatT>::propagationFunction(const SystemModelSE2::StateT& state,
+ const SystemModelSE2::ControlT& control,
+ const SystemModelSE2::ControlNoiseT& noise,
+ FloatT dt)
+ {
+ StateT new_state;
+ new_state.pose = state.pose.template rplus(control.velocity * dt);
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2<floatT>::StateT
+ SystemModelSE2<floatT>::retraction(const SystemModelSE2::StateT& state,
+ const SystemModelSE2::SigmaPointsT& sigmaPoints)
+ {
+ StateT new_state;
+ manif::SE2Tangent<FloatT> tan;
+ tan.coeffs() << sigmaPoints;
+ new_state.pose = state.pose.lplus(tan);
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2<floatT>::SigmaPointsT
+ SystemModelSE2<floatT>::inverseRetraction(const SystemModelSE2::StateT& state1,
+ const SystemModelSE2::StateT& state2)
+ {
+ SigmaPointsT sigma;
+ sigma = state2.pose.lminus(state1.pose).coeffs();
+ return sigma;
+ }
+
+ template struct SystemModelSE2<float>;
+ template struct SystemModelSE2<double>;
+
+
+ // ------------------------------ SE2xV -----------------------------------------
+
+ template <typename floatT>
+ typename SystemModelSE2xV<floatT>::ObsT
+ SystemModelSE2xV<floatT>::observationFunction(const SystemModelSE2xV::StateT& state)
+ {
+ ObsT observation = state.pose.log().coeffs();
+ return observation;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2xV<floatT>::StateT
+ SystemModelSE2xV<floatT>::propagationFunction(const SystemModelSE2xV::StateT& state,
+ const SystemModelSE2xV::ControlT& control,
+ const SystemModelSE2xV::ControlNoiseT& noise,
+ FloatT dt)
+ {
+ StateT new_state;
+ new_state.pose = state.pose.template rplus(state.velocity * dt);
+ new_state.velocity = state.velocity;
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2xV<floatT>::StateT
+ SystemModelSE2xV<floatT>::retraction(const SystemModelSE2xV::StateT& state,
+ const SystemModelSE2xV::SigmaPointsT& sigmaPoints)
+ {
+ StateT new_state;
+ manif::SE2Tangent<FloatT> tan;
+ tan.coeffs() << sigmaPoints.segment(0, 3);
+ new_state.pose = state.pose.lplus(tan);
+ tan.coeffs() << sigmaPoints.segment(3, 3);
+ // TODO: this is probably not correct, i.e. there needs to be some interaction between velocity and pose
+ new_state.velocity += tan;
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2xV<floatT>::SigmaPointsT
+ SystemModelSE2xV<floatT>::inverseRetraction(const SystemModelSE2xV::StateT& state1,
+ const SystemModelSE2xV::StateT& state2)
+ {
+ SigmaPointsT sigma;
+ sigma.segment(0, 3) = state2.pose.lminus(state1.pose).coeffs();
+ // TODO: this is probably not correct; probably one cannot subtract two tangent vectors at two different poses
+ // from each other
+ sigma.segment(3, 3) = (state2.velocity - state1.velocity).coeffs();
+ return sigma;
+ }
+
+ template struct SystemModelSE2xV<float>;
+ template struct SystemModelSE2xV<double>;
+
+} // namespace armarx::navigation::human::kalman_filter
diff --git a/source/armarx/navigation/human/HumanSystemModel.h b/source/armarx/navigation/human/HumanSystemModel.h
new file mode 100644
index 0000000000000000000000000000000000000000..05de1fdcecb71f274a8b3ce26fa88cbcaee575c6
--- /dev/null
+++ b/source/armarx/navigation/human/HumanSystemModel.h
@@ -0,0 +1,171 @@
+/**
+ * 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 <manif/SE2.h>
+#include <manif/SO2.h>
+
+namespace armarx::navigation::human::kalman_filter
+{
+ //----------- SO2xR2 -----------------
+
+ template <typename floatT>
+ struct StateSO2xR2
+ {
+ manif::SO2<floatT> orientation;
+ Eigen::Matrix<floatT, 2, 1> position;
+ };
+
+ template <typename floatT>
+ struct ControlSO2xR2
+ {
+ typename manif::SO2<floatT>::Tangent angular_velocity;
+ Eigen::Matrix<floatT, 2, 1> euclidean_velocity;
+ };
+
+ template <typename floatT>
+ struct SystemModelSO2xR2
+ {
+ static_assert(std::is_floating_point_v<floatT>);
+ struct dim
+ {
+ static constexpr long state = 3, control = 3, obs = 3;
+ };
+
+ using FloatT = floatT;
+ using StateT = StateSO2xR2<FloatT>;
+ using ControlT = ControlSO2xR2<FloatT>;
+ using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+ using ControlNoiseT = Eigen::
+ Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
+
+ static ObsT observationFunction(const StateT& state);
+
+ static StateT propagationFunction(const StateT& state,
+ const ControlT& control,
+ const ControlNoiseT& noise,
+ FloatT dt);
+
+ static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
+
+ static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
+ };
+
+ extern template struct SystemModelSO2xR2<float>;
+ extern template struct SystemModelSO2xR2<double>;
+
+
+ //----------- SE2 -----------------
+
+ template <typename floatT>
+ struct StateSE2
+ {
+ manif::SE2<floatT> pose;
+ };
+
+ template <typename floatT>
+ struct ControlSE2
+ {
+ typename manif::SE2<floatT>::Tangent velocity;
+ };
+
+ template <typename floatT>
+ struct SystemModelSE2
+ {
+ static_assert(std::is_floating_point_v<floatT>);
+ struct dim
+ {
+ static constexpr long state = 3, control = 3, obs = 3;
+ };
+
+ using FloatT = floatT;
+ using StateT = StateSE2<FloatT>;
+ using ControlT = ControlSE2<FloatT>;
+ using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+ using ControlNoiseT = Eigen::
+ Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
+
+ static ObsT observationFunction(const StateT& state);
+
+ static StateT propagationFunction(const StateT& state,
+ const ControlT& control,
+ const ControlNoiseT& noise,
+ FloatT dt);
+
+ static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
+
+ static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
+ };
+
+ extern template struct SystemModelSE2<float>;
+ extern template struct SystemModelSE2<double>;
+
+
+ //----------- SE2xV -----------------
+
+ template <typename floatT>
+ struct StateSE2xV
+ {
+ manif::SE2<floatT> pose;
+ typename manif::SE2<floatT>::Tangent velocity;
+ };
+
+ template <typename floatT>
+ struct ControlSE2xV
+ {
+ };
+
+ template <typename floatT>
+ struct SystemModelSE2xV
+ {
+ static_assert(std::is_floating_point_v<floatT>);
+ struct dim
+ {
+ static constexpr long state = 6, control = 1, obs = 3;
+ };
+
+ using FloatT = floatT;
+ using StateT = StateSE2xV<FloatT>;
+ using ControlT = ControlSE2xV<FloatT>;
+ using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+ using ControlNoiseT = Eigen::
+ Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
+
+ static ObsT observationFunction(const StateT& state);
+
+ static StateT propagationFunction(const StateT& state,
+ const ControlT& control,
+ const ControlNoiseT& noise,
+ FloatT dt);
+
+ static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
+
+ static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
+ };
+
+ extern template struct SystemModelSE2xV<float>;
+ extern template struct SystemModelSE2xV<double>;
+
+} // namespace armarx::navigation::human::kalman_filter
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7273cdd723e0ff9e6bd6ac98125418a3a4b904c7
--- /dev/null
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -0,0 +1,384 @@
+#include "HumanTracker.h"
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include <armarx/navigation/human/types.h>
+#include <armarx/navigation/conversions/eigen.h>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+
+
+namespace armarx::navigation::human
+{
+
+ void
+ HumanTracker::update(const CameraMeasurement& measurements)
+ {
+ prepareTrackedHumans(measurements.detectionTime);
+
+ // calculate the poses according to the new received measurements
+ std::vector<DetectedHuman> newPoses =
+ measurements.humanPoses |
+ ranges::views::transform(
+ [measurements, this](const armem::human::HumanPose& humanPose) -> DetectedHuman {
+ return convertHumanPoseToDetectedHuman(measurements.detectionTime, humanPose);
+ }) |
+ ranges::to_vector;
+
+ // associate the new poses from the measurement with the tracked humans
+ associateHumans(newPoses);
+
+ // add all not associated poses as new tracked humans
+ for (const auto& detectedHuman : newPoses)
+ {
+ if (not detectedHuman.associated)
+ {
+ //add new tracked human to list of tracked humans
+ trackedHumans.push_back(TrackedHuman{
+ .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime},
+ .trackingId = detectedHuman.trackingId,
+ .associatedCluster = nullptr,
+ .associated = true});
+ }
+ }
+ }
+
+ std::vector<Cluster>
+ HumanTracker::update(const LaserMeasurement& measurements)
+ {
+
+ prepareTrackedHumans(measurements.detectionTime);
+
+ // only look at the clusters that have a size reasonable enough to contain at most two
+ // footprints
+ std::vector<AdvancedCluster> potentialFootprints;
+ std::vector<Cluster> unusedClusters;
+ for (const auto& cluster : measurements.clusters)
+ {
+ if (getClusterSize(cluster) <= 2 * parameters.maxFootprintSize)
+ {
+ potentialFootprints.push_back(AdvancedCluster{
+ .center = getClusterCenter(cluster), .cluster = cluster, .associated = false});
+ }
+ else
+ {
+ unusedClusters.push_back(cluster);
+ }
+ }
+
+ // associate humans and clusters by their distances
+ const auto sortedDistances = getSortedDistances(trackedHumans, potentialFootprints);
+
+ for (auto& posDistance : sortedDistances)
+ {
+ if (posDistance.distance > parameters.maxAssociationDistance)
+ {
+ break;
+ }
+ if (posDistance.oldHuman->associated || posDistance.newCluster->associated)
+ {
+ continue;
+ }
+ // no footprint was found before, so associate first footprint
+ if (!posDistance.oldHuman->associatedCluster)
+ {
+ posDistance.oldHuman->associatedCluster = posDistance.newCluster;
+ posDistance.newCluster->associated = true;
+ }
+ // found both footprints, so associate both footprints and update human with new pose
+ else
+ {
+ //calculate center between both footprints
+ auto& footprint1 = posDistance.oldHuman->associatedCluster->cluster;
+ auto& footprint2 = posDistance.newCluster->cluster;
+
+ //create pose with orientation from old human
+ core::Pose2D pose = core::Pose2D::Identity();
+ pose.translation() =
+ (getClusterCenter(footprint1) + getClusterCenter(footprint2)) / 2;
+ pose.linear() = posDistance.oldHuman->humanFilter.get().pose.linear();
+
+
+ posDistance.oldHuman->humanFilter.update(pose, measurements.detectionTime);
+
+ posDistance.oldHuman->associatedCluster = nullptr;
+ posDistance.oldHuman->associated = true;
+ posDistance.newCluster->associated = true;
+ }
+ }
+
+ //associate humans where only one footprint was found
+ for (auto& human : trackedHumans)
+ {
+ if (human.associated)
+ {
+ continue;
+ }
+
+ if (human.associatedCluster)
+ {
+ auto& footprint = human.associatedCluster;
+ //create pose with orientation from old human
+ core::Pose2D pose = core::Pose2D::Identity();
+ pose.translation() = footprint->center;
+ pose.linear() = human.humanFilter.get().pose.linear();
+
+ human.humanFilter.update(pose, measurements.detectionTime);
+
+ human.associatedCluster = nullptr;
+ human.associated = true;
+ }
+ }
+
+ // add unused clusters to return list
+ for (auto& cluster : potentialFootprints)
+ {
+ if (!cluster.associated)
+ {
+ unusedClusters.push_back(cluster.cluster);
+ }
+ }
+ return unusedClusters;
+ }
+
+ std::vector<human::Human>
+ HumanTracker::getTrackedHumans() const
+ {
+ return trackedHumans |
+ ranges::views::transform([](const TrackedHuman& h) -> human::Human
+ { return h.humanFilter.get(); }) |
+ ranges::to_vector;
+ }
+
+ void
+ HumanTracker::reset()
+ {
+ trackedHumans.clear();
+ }
+
+
+ HumanTracker::DetectedHuman
+ HumanTracker::convertHumanPoseToDetectedHuman(const DateTime& time,
+ const armem::human::HumanPose& humanPose)
+ {
+ const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
+ ARMARX_CHECK_NOT_EMPTY(keypoints);
+
+ // calculate the arithmetic mean of all keypoint positions
+ Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
+ int size = 0;
+ for (const auto& [_, v] : keypoints)
+ {
+ if (v.positionGlobal.has_value())
+ {
+ centerPos += v.positionGlobal.value().toEigen();
+ size++;
+ }
+ }
+ centerPos /= size;
+
+ // calculate the yaw of the specified keypoint representing the orientation if it exists
+ double yaw = 0;
+ auto head = humanPose.keypoints.find(parameters.rotationKeypoint);
+ if (head != humanPose.keypoints.end())
+ {
+ const armem::human::PoseKeypoint& refKeypoint = head->second;
+ ARMARX_CHECK(refKeypoint.orientationGlobal.has_value());
+ Eigen::Quaternionf qhead = refKeypoint.orientationGlobal->toEigenQuaternion();
+ Eigen::Vector3f vec(1, 0, 0);
+ Eigen::Vector3f rot3 = qhead._transformVector(vec);
+ Eigen::Vector2f rot2(rot3.x(), rot3.y());
+ if (rot2.norm() != 0)
+ {
+ // calculate angle of rot2
+ yaw = atan2(rot2.y(), rot2.x());
+ }
+ }
+
+ // create the new pose with the calculated position and yaw
+ core::Pose2D pose = core::Pose2D::Identity();
+ pose.translation() = conv::to2D(centerPos);
+ pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
+
+ return {pose, humanPose.humanTrackingId, time, false};
+ }
+
+ std::vector<HumanTracker::PosHumanDistance>
+ HumanTracker::getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ std::vector<HumanTracker::DetectedHuman>& newHumans)
+ {
+ std::vector<PosHumanDistance> posDistances;
+
+ for (auto& oldHuman : oldHumans)
+ {
+ if (oldHuman.associated)
+ {
+ continue;
+ }
+ for (auto& newHuman : newHumans)
+ {
+ if (newHuman.associated)
+ {
+ continue;
+ }
+ // calculate distance between every possible combination of tracked and detected
+ // humans where none of them was associated
+ posDistances.push_back(
+ {&oldHuman,
+ &newHuman,
+ (newHuman.pose.translation() - oldHuman.humanFilter.get().pose.translation())
+ .norm()});
+ }
+ }
+
+ // sort the distances ascending by their numeric value
+ std::sort(posDistances.begin(),
+ posDistances.end(),
+ [](const PosHumanDistance& a, const PosHumanDistance& b) -> bool
+ { return a.distance < b.distance; });
+
+ return posDistances;
+ }
+
+ std::vector<HumanTracker::PosLaserDistance>
+ HumanTracker::getSortedDistances(std::vector<TrackedHuman>& oldHumans,
+ std::vector<AdvancedCluster>& newClusters)
+ {
+ std::vector<PosLaserDistance> posDistances;
+
+ for (auto& oldHuman : oldHumans)
+ {
+ for (auto& newCluster : newClusters)
+ {
+ // calculate distance between every possible combination of tracked human and cluster
+ posDistances.push_back(
+ {&oldHuman,
+ &newCluster,
+ (newCluster.center - oldHuman.humanFilter.get().pose.translation()).norm()});
+ }
+ }
+
+ // sort the distances ascending by their numeric value
+ std::sort(posDistances.begin(),
+ posDistances.end(),
+ [](const PosLaserDistance& a, const PosLaserDistance& b) -> bool
+ { return a.distance < b.distance; });
+
+ return posDistances;
+ }
+
+ void
+ HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
+ {
+ // first, associate humans by their tracking id
+ for (auto& oldHuman : trackedHumans)
+ {
+ if (oldHuman.associated || not oldHuman.trackingId)
+ {
+ continue;
+ }
+ for (auto& newHuman : detectedHumans)
+ {
+ if (newHuman.associated || not newHuman.trackingId)
+ {
+ continue;
+ }
+ // check for every possible combination of old and new humans (that were not already
+ // associated and have a tracking id) if their tracking id is the same
+ if (oldHuman.trackingId.value() == newHuman.trackingId.value())
+ {
+ associate(&oldHuman, &newHuman);
+ }
+ }
+ }
+
+ // second, associate leftover humans by their distances
+ const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
+
+ for (auto& posDistance : sortedDistances)
+ {
+ if (posDistance.distance > parameters.maxAssociationDistance)
+ {
+ break;
+ }
+ if (posDistance.oldHuman->associated || posDistance.newHuman->associated)
+ {
+ continue;
+ }
+ // associate the pair with the currently smallest distance between non-associated humans
+ associate(posDistance.oldHuman, posDistance.newHuman);
+ }
+ }
+
+ void
+ HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
+ {
+ ARMARX_CHECK(not trackedHuman->associated);
+ ARMARX_CHECK(not detectedHuman->associated);
+
+
+ trackedHuman->associated = true;
+ detectedHuman->associated = true;
+
+ trackedHuman->humanFilter.update(detectedHuman->pose, detectedHuman->detectionTime);
+ trackedHuman->trackingId = detectedHuman->trackingId;
+ }
+
+ float
+ HumanTracker::getClusterSize(Cluster cluster)
+ {
+ return 2 * cluster.circle.radius;
+ }
+
+ Eigen::Vector2f
+ HumanTracker::getClusterCenter(Cluster cluster)
+ {
+ typedef boost::geometry::model::d2::point_xy<double> boost_point;
+ typedef boost::geometry::model::polygon<boost_point> boost_polygon;
+
+ ARMARX_CHECK(!cluster.convexHull.empty());
+ std::vector<boost_point> points;
+ for (auto& point : cluster.convexHull)
+ {
+ points.push_back(boost_point(point.x(), point.y()));
+ }
+ //make polygon enclosed
+ points.push_back(
+ boost_point(cluster.convexHull.front().x(), cluster.convexHull.front().y()));
+ boost_polygon poly;
+ boost::geometry::assign_points(poly, points);
+
+ boost_point p;
+ boost::geometry::centroid(poly, p);
+
+ return Eigen::Vector2f{p.x(), p.y()};
+ }
+
+ void
+ HumanTracker::prepareTrackedHumans(DateTime time)
+ {
+ // iterate over all existing tracked humans
+ for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
+ {
+ auto& human = *it;
+ // when the tracked human recieved no new measurement for too long, remove it
+ if (human.humanFilter.detectionAge(time) >= parameters.maxTrackingAge)
+ {
+ it = trackedHumans.erase(it);
+ }
+ // otherwise the tracked human is prepared for association at the current point in time
+ else
+ {
+ human.associatedCluster = nullptr;
+ human.associated = false;
+ human.humanFilter.propagation(time);
+ it++;
+ }
+ }
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
new file mode 100644
index 0000000000000000000000000000000000000000..1f27aed75375018ce613579ee07f4577564b4249
--- /dev/null
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -0,0 +1,222 @@
+/**
+ * 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>
+
+#include <RobotAPI/libraries/armem_vision/types.h>
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <VisionX/libraries/armem_human/types.h>
+
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/HumanFilter.h>
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::human
+{
+ using Cluster = armem::vision::LaserScannerFeature;
+
+
+ /**
+ * @brief The HumanTracker class can be used to track and filter multiple humans. It hides
+ * implementation detail on how new detected humans are associated to the old, already tracked
+ * humans. New detected humans can be fed by using the update method. The tracked humans can
+ * be obtained using the getTrackedHumans method.
+ */
+ class HumanTracker
+ {
+ public:
+ HumanTracker() = default;
+
+ struct CameraMeasurement
+ {
+ DateTime detectionTime;
+ std::vector<armem::human::HumanPose> humanPoses;
+ };
+
+ struct LaserMeasurement
+ {
+ DateTime detectionTime;
+ std::vector<Cluster> clusters;
+ };
+
+ struct DetectedHuman
+ {
+ core::Pose2D pose;
+ std::optional<std::string> trackingId;
+ DateTime detectionTime;
+ bool associated;
+ };
+
+ struct AdvancedCluster
+ {
+ Eigen::Vector2f center;
+ Cluster cluster;
+ bool associated;
+ };
+
+ struct TrackedHuman
+ {
+ HumanFilter humanFilter;
+ std::optional<std::string> trackingId = std::nullopt;
+ AdvancedCluster* associatedCluster;
+ bool associated;
+ };
+
+ struct PosHumanDistance
+ {
+ HumanTracker::TrackedHuman* oldHuman;
+ HumanTracker::DetectedHuman* newHuman;
+ float distance;
+ };
+
+ struct PosLaserDistance
+ {
+ HumanTracker::TrackedHuman* oldHuman;
+ AdvancedCluster* newCluster;
+ float distance;
+ };
+
+ struct Parameters
+ {
+ // the keypoint which should be used for calculating the rotation of the humans
+ const std::string rotationKeypoint = "Head";
+ // the duration after which tracked humans will be erased if no new measurement for
+ // this human is found
+ Duration maxTrackingAge = Duration::MilliSeconds(1000);
+ // the maximum distance in millimeters of two human measurements where they are still
+ // associated with each other
+ float maxAssociationDistance = 600;
+ // the maximum size (aka length) in millimeters a footprint can have
+ float maxFootprintSize = 350;
+ // alpha value from interval [0,1] to determine how much the current (and respectively
+ // the old) velocity should be weighted when calculating the new velocity
+ float velocityAlpha = 0.7;
+ };
+ /**
+ * @brief HumanTracker::update Updates the tracked humans with the human measurements from a camera. When a
+ * measurement is close enough to an existing tracked human, they are associated, otherwise a
+ * new tracked human is created. Tracked humans that were not associated with a new measurement
+ * for a specified amount of time are removed. New associated measurements for a tracked human
+ * are always filtered to provide a less noisy state.
+ * @param measurements the new measurements of the environment
+ */
+ void update(const CameraMeasurement& measurements);
+ /**
+ * @brief HumanTracker::update Updates the tracked humans with the measurements from a lasersensor.
+ * When a measurement is close enough to an existing tracked human, they are associated,
+ * otherwise the measurement is not used for human tracking but returned in the list of unused
+ * measurements. Tracked humans that were not associated with a new measurement for a
+ * specified amount of time are removed. New associated measurements for a tracked human
+ * are always filtered to provide a less noisy state.
+ * @param measurements
+ * @return
+ */
+ std::vector<Cluster> update(const LaserMeasurement& measurements);
+ /**
+ * @brief HumanTracker::getTrackedHumans Returns all humans that are currently tracked.
+ * @return the tracked humans
+ */
+ std::vector<human::Human> getTrackedHumans() const;
+ /**
+ * @brief HumanTracker::reset Resets this instance to the same state as if it just would have
+ * been created.
+ */
+ void reset();
+
+ private:
+ /**
+ * @brief convertHumanPoseToDetectedHuman Sets every parameter of a detected human according
+ * to the human pose and returns the new created detected human.
+ * @param time the time of detection
+ * @param humanPose the human pose representing the human
+ * @return the new created detected human
+ */
+ DetectedHuman convertHumanPoseToDetectedHuman(const DateTime& time,
+ const armem::human::HumanPose& humanPose);
+ /**
+ * @brief getSortedDistances Returns a sorted vector of the distances between every possible
+ * combination (T, D) where T is an old, tracked human and D is a new, detected human and
+ * both of them are not already associated. The smallest distance will be the first entry in
+ * the vector.
+ * @param oldHumans the old, tracked humans
+ * @param newHumans the new, detected humans
+ * @return the sorted vector of distances with references to the according humans
+ */
+ std::vector<PosHumanDistance>
+ getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ std::vector<HumanTracker::DetectedHuman>& newHumans);
+ /**
+ * @brief getSortedDistances Returns a sorted vector of the distances between every possible
+ * combination (T, C) where T is an old, tracked human and C is a new, detected cluster.
+ * The smallest distance will be the first entry in the vector.
+ * @param oldHumans
+ * @param newClusters
+ * @return
+ */
+ std::vector<PosLaserDistance>
+ getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ std::vector<AdvancedCluster>& newClusters);
+ /**
+ * @brief HumanTracker::associateHumans Associates those tracked and detected humans that
+ * belong together.
+ * @param detectedHumans The detected humans against which the saved list of tracked humans is
+ * matched.
+ */
+ void associateHumans(std::vector<DetectedHuman>& detectedHumans);
+ /**
+ * @brief HumanTracker::associate Associates the given tracked and detected human. Therefore it
+ * updates all necessary variables of the TrackedHuman
+ * @param trackedHuman the tracked human
+ * @param detectedHuman the detected human
+ */
+ void associate(TrackedHuman* tracked, DetectedHuman* detected);
+ /**
+ * @brief getClusterSize Returns the size of the given cluster. That is for now the maximum
+ * distance between two points.
+ * @param cluster The cluster whose size is measured
+ * @return the size
+ */
+ float getClusterSize(Cluster cluster);
+ /**
+ * @brief HumanTracker::getClusterCenter Returns the center of the given cluster. That is
+ * calculated as the mean of all points.
+ * @param cluster The cluster whose center is calculated
+ * @return the center
+ */
+ Eigen::Vector2f getClusterCenter(Cluster cluster);
+ /**
+ * @brief prepareTrackedHumans Deletes all tracked humans that received no update for too
+ * long. All remaining tracked humans are prepared for association with the given point in
+ * time.
+ */
+ void prepareTrackedHumans(DateTime time);
+
+
+ private:
+ std::vector<TrackedHuman> trackedHumans;
+ Parameters parameters;
+ };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.cpp b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..acd453fa124371ac38a5438bed46386fc7f6978a
--- /dev/null
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
@@ -0,0 +1,52 @@
+#include "ProxemicZoneCreator.h"
+
+#include <cmath>
+
+namespace armarx::navigation::human
+{
+
+ ProxemicZones
+ ProxemicZoneCreator::createProxemicZones(const Human& human)
+ {
+ // intimate zone
+ ProxemicZone intimate{
+ .pose = human.pose,
+ .shape = {.a = params.intimateRadius, .b = params.intimateRadius},
+ .penalty = intimatePenalty,
+ .weight = params.intimateWeight,
+ .homotopicRelevance = true,
+ };
+
+ // personal zone
+ 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();
+ // stretch with velocity in m/s as factor
+ float movementStretch = 1 + velocityNorm * params.movementInfluence / 1000; // [mm] to [m]
+ if (velocityNorm != 0)
+ {
+ // y pointing forward
+ float movementDirection =
+ std::atan2(human.linearVelocity.y(), human.linearVelocity.x()) - M_PI_2;
+ pose.linear() = Eigen::Rotation2Df(movementDirection).toRotationMatrix();
+ pose.translation() +=
+ human.linearVelocity / velocityNorm * params.personalRadius * (movementStretch - 1);
+ }
+
+ ProxemicZone personal{
+ .pose = pose,
+ .shape = {.a = params.personalRadius, .b = movementStretch * params.personalRadius},
+ .penalty = personalPenalty,
+ .weight = params.personalWeight,
+ .homotopicRelevance = false,
+ };
+
+ return {intimate, personal};
+ }
+
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.h b/source/armarx/navigation/human/ProxemicZoneCreator.h
new file mode 100644
index 0000000000000000000000000000000000000000..fed4c18cce4105e0ffba8a6023bcb68268ae4b41
--- /dev/null
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.h
@@ -0,0 +1,69 @@
+/**
+ * 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 )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::human
+{
+
+ class ProxemicZoneCreator
+ {
+
+ public:
+ struct ProxemicParameters
+ {
+ float intimateWeight = 2.0;
+ float personalWeight = 1.0;
+
+ float intimateRadius = 400;
+ float personalRadius = 1000;
+
+ float movementInfluence = 1;
+ // 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};
+ };
+
+
+ ProxemicZoneCreator()
+ {
+ intimatePenalty = {.minDistance = 0.1, .epsilon = 0, .exponent = 1.2};
+ personalPenalty = {.minDistance = 0.1, .epsilon = 0, .exponent = 1.2};
+ }
+ ProxemicZoneCreator(ProxemicParameters& params,
+ ExponentialPenaltyModel& intimatePenalty,
+ ExponentialPenaltyModel& personalPenalty) :
+ params(params), intimatePenalty(intimatePenalty), personalPenalty(personalPenalty)
+ {
+ }
+
+
+ ProxemicZones createProxemicZones(const Human& human);
+
+ private:
+ ProxemicParameters params;
+ ExponentialPenaltyModel intimatePenalty;
+ ExponentialPenaltyModel personalPenalty;
+ };
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp b/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5818d6edc6e852bc991b6fbcd9d752eeee1013f9
--- /dev/null
+++ b/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp
@@ -0,0 +1,14 @@
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp>
+
+#include <armarx/navigation/human/HumanSystemModel.h>
+
+
+template class UnscentedKalmanFilter<
+ armarx::navigation::human::kalman_filter::SystemModelSO2xR2<float>>;
+template class UnscentedKalmanFilter<
+ armarx::navigation::human::kalman_filter::SystemModelSO2xR2<double>>;
+
+template class UnscentedKalmanFilter<
+ armarx::navigation::human::kalman_filter::SystemModelSE2<double>>;
+template class UnscentedKalmanFilter<
+ armarx::navigation::human::kalman_filter::SystemModelSE2xV<double>>;
diff --git a/source/armarx/navigation/human/aron/Human.xml b/source/armarx/navigation/human/aron/Human.xml
new file mode 100644
index 0000000000000000000000000000000000000000..57d104d858b8b346531c61b73069aba168ce374d
--- /dev/null
+++ b/source/armarx/navigation/human/aron/Human.xml
@@ -0,0 +1,34 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name='armarx::navigation::human::arondto::Human'>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='linearVelocity'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='detectionTime'>
+ <Time />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::human::arondto::HumanGroup'>
+ <ObjectChild key='shape'>
+ <List>
+ <Position />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='humans'>
+ <List>
+ <armarx::navigation::human::arondto::Human />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='detectionTime'>
+ <Time />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/human/aron_conversions.cpp b/source/armarx/navigation/human/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e09a3ca376d5f75790c8adada1038e627c968161
--- /dev/null
+++ b/source/armarx/navigation/human/aron_conversions.cpp
@@ -0,0 +1,69 @@
+#include "aron_conversions.h"
+
+#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/types.h>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+
+namespace armarx::navigation::human
+{
+ void
+ toAron(arondto::Human& dto, const Human& bo)
+ {
+ dto.pose = conv::to3D(bo.pose).matrix();
+ dto.linearVelocity = conv::to3D(bo.linearVelocity);
+ dto.detectionTime = bo.detectionTime;
+ }
+
+ void
+ fromAron(const arondto::Human& dto, Human& bo)
+ {
+ bo.pose = conv::to2D(core::Pose(dto.pose));
+ bo.linearVelocity = conv::to2D(dto.linearVelocity);
+ bo.detectionTime = dto.detectionTime;
+ }
+
+
+ void
+ toAron(arondto::HumanGroup& dto, const HumanGroup& bo)
+ {
+ dto.shape = bo.shape.vertices |
+ ranges::views::transform([](const Eigen::Vector2f& boVer) -> Eigen::Vector3f
+ { return conv::to3D(boVer); }) |
+ ranges::to_vector;
+
+ dto.humans = bo.humans |
+ ranges::views::transform(
+ [](const Human& boHuman) -> arondto::Human
+ {
+ arondto::Human dtoHuman;
+ toAron(dtoHuman, boHuman);
+ return dtoHuman;
+ }) |
+ ranges::to_vector;
+ dto.detectionTime = bo.detectionTime;
+ }
+
+ void
+ fromAron(const arondto::HumanGroup& dto, HumanGroup& bo)
+ {
+ bo.shape.vertices =
+ dto.shape |
+ ranges::views::transform([](const Eigen::Vector3f& dtoVer) -> Eigen::Vector2f
+ { return conv::to2D(dtoVer); }) |
+ ranges::to_vector;
+
+ bo.humans = dto.humans |
+ ranges::views::transform(
+ [](const arondto::Human& dtoHuman) -> Human
+ {
+ Human boHuman;
+ fromAron(dtoHuman, boHuman);
+ return boHuman;
+ }) |
+ ranges::to_vector;
+ bo.detectionTime = dto.detectionTime;
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/aron_conversions.h b/source/armarx/navigation/human/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..71159748a75ed5d44312650a530c80cc2e25e1ab
--- /dev/null
+++ b/source/armarx/navigation/human/aron_conversions.h
@@ -0,0 +1,46 @@
+/**
+ * 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 )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+namespace armarx::navigation::human
+{
+ struct Human;
+ struct HumanGroup;
+
+ namespace arondto
+ {
+ struct Human;
+ struct HumanGroup;
+
+ } // namespace arondto
+
+} // namespace armarx::navigation::human
+
+namespace armarx::navigation::human
+{
+ void toAron(arondto::Human& dto, const Human& bo);
+ void fromAron(const arondto::Human& dto, Human& bo);
+
+ void toAron(arondto::HumanGroup& dto, const HumanGroup& bo);
+ void fromAron(const arondto::HumanGroup& dto, HumanGroup& bo);
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/shapes.h b/source/armarx/navigation/human/shapes.h
new file mode 100644
index 0000000000000000000000000000000000000000..f6fd25a5156684648c7abf4139ebe04f27188a32
--- /dev/null
+++ b/source/armarx/navigation/human/shapes.h
@@ -0,0 +1,48 @@
+/**
+ * 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 )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/shapes.h>
+
+#include <armarx/navigation/core/basic_types.h>
+
+namespace armarx::navigation::human::shapes
+{
+
+ /**
+ * @brief An axis oriented ellipse with half-axes a and b along the x- and y-axis respectively.
+ */
+ struct Ellipse
+ {
+ float a;
+ float b;
+ };
+
+ /**
+ * @brief A polygon with arbitrarily many vertices. The polygon will always be closed automatically.
+ */
+ struct Polygon
+ {
+ std::vector<Eigen::Vector2f> vertices;
+ };
+
+} // namespace armarx::navigation::human::shapes
diff --git a/source/armarx/navigation/human/test/UnscentedKalmanFilterTest.cpp b/source/armarx/navigation/human/test/UnscentedKalmanFilterTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bc1ec3847e849a6383cdee4f4f9e3bc9ef9a9542
--- /dev/null
+++ b/source/armarx/navigation/human/test/UnscentedKalmanFilterTest.cpp
@@ -0,0 +1,322 @@
+/*
+ * 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 ROBDEKON
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 18.03.21
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *
+ * Code adapted to C++ from: https://github.com/CAOR-MINES-ParisTech/ukfm
+ * See also:
+ * - https://arxiv.org/pdf/2002.00878.pdf
+ * - https://ieeexplore.ieee.org/document/8206066/
+ */
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/time.h>
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/Test.h>
+#include <sciplot/sciplot.hpp>
+
+using T = double;
+using Vector = Eigen::Matrix<T, 3, 1>;
+using Matrix = Eigen::Matrix<T, 3, 3>;
+using SystemModelT = SystemModelSE3<T>;
+
+constexpr long num_timesteps = 3000;
+constexpr T max_time = 1;
+constexpr T dt = max_time / num_timesteps;
+constexpr T c = (1 / max_time) * 2 * M_PI;
+
+constexpr T acc_noise_std = 0.01;
+constexpr T om_noise_std = 0.01;
+constexpr T obs_noise_std = 0.01;
+constexpr T initial_offset_angle = 0.0 * 10 * M_PI / 180;
+const Vector initial_offet_pos = 0.5 * Vector(1, 0.5, 0.7);
+
+void
+test_retract()
+{
+ for (int i = 0; i < num_timesteps; i++)
+ {
+ SystemModelT::StateT state1, state2;
+ state1.pose = manif::SE3<T>::Random();
+ // state1.velocity.setRandom();
+ state2.pose = manif::SE3<T>::Random();
+ // state2.velocity.setRandom();
+ // sigma = state2 - state1
+ SystemModelT::SigmaPointsT sigma = SystemModelT::inverseRetraction(state1, state2);
+ // state3 = state1 + sigma --> state3 = state2
+ SystemModelT::StateT state3 = SystemModelT::retraction(state1, sigma);
+ // ARMARX_CHECK((state2.velocity - state3.velocity).norm() < 10 * std::numeric_limits<T>::epsilon());
+ ARMARX_CHECK((state2.pose - state3.pose).coeffs().norm() <
+ 10000 * std::numeric_limits<T>::epsilon())
+ << (state2.pose - state3.pose).coeffs().norm();
+
+ SystemModelT::SigmaPointsT sigma2 = SystemModelT::inverseRetraction(state1, state3);
+ // TODO: fix bad accuracy
+ ARMARX_CHECK((sigma2 - sigma).norm() < 10000 * std::numeric_limits<T>::epsilon())
+ << (sigma2 - sigma).norm();
+ }
+}
+
+void
+test_se3()
+{
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T angle = t * c;
+ const Eigen::Matrix<T, 3, 1> pos(std::sin(angle), std::cos(angle), 0);
+ manif::SO3<T> rot = manif::SO3<T>(0, 0, angle);
+ ARMARX_CHECK(std::abs(rot.log().coeffs()(2) - angle) < 1e-6);
+ manif::SE3<T> pose(pos, rot);
+
+ manif::SE3Tangent<T> tangent = pose.log();
+ ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 1e-6);
+ ARMARX_CHECK(std::abs(tangent.coeffs()(5) - angle) < 1e-6);
+ }
+}
+
+void
+simulate_trajectory(std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ControlT>& omegas)
+{
+ SystemModelT::StateT state;
+ // state.velocity = Vector(c, 0, 0);
+ state.pose = manif::SE3<T>(0, 1, 0, M_PI, M_PI, 0);
+ states.push_back(state);
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T t_prev = dt * (i - 1);
+ const T angle = t * c;
+ // yaw goes from -pi to pi
+ const T yaw = 1 * M_PI * std::sin(angle);
+ const T roll = 0.1 * M_PI * std::sin(angle) + M_PI;
+ const T pitch = 0.1 * M_PI * std::sin(angle) + M_PI;
+ const Vector pos(std::sin(angle), std::cos(angle), 0);
+ manif::SE3Tangent<T> tangent(states.at(i - 1).pose.log());
+ ARMARX_CHECK((states.at(i - 1).pose.translation() - pos).norm() < 0.1);
+ // ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 0.1);
+ const Vector vel(std::cos(angle) * c, -std::sin(angle) * c, 0);
+ const Vector acc(-std::sin(angle) * (c * c), -std::cos(angle) * (c * c), 0);
+ const Vector acc_prev(-std::sin(t_prev * c) * (c * c), -std::cos(t_prev * c) * (c * c), 0);
+ // const Eigen::Vector3f vel((pos - state.position) / dt);
+ // Eigen::Vector3f acc = (vel - state.velocity) / dt;
+ manif::SO3<T> rot = manif::SO3<T>(roll, pitch, yaw);
+ // Eigen::Matrix<T, 3, 1> rot_tan = rot.log().coeffs();
+ // ARMARX_CHECK(std::abs(rot_tan(2) - yaw) < 1e-6);
+ SystemModelT::ControlT control;
+ // control.linear_accel = state.pose.rotation().inverse() * acc_prev;
+ Vector angular_velocity(0.1 * M_PI * std::cos(angle) * c,
+ 0.1 * M_PI * std::cos(angle) * c,
+ M_PI * std::cos(angle) * c);
+ control.velocity.coeffs() << state.pose.asSO3().inverse().act(vel), angular_velocity;
+
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
+ state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+ manif::SE3Tangent<T> pro_tangent = propagated.pose.log();
+ T pos_diff1 = (propagated.pose.translation() - pos).norm();
+ // ARMARX_CHECK(pos_diff1 < pos_diff2);
+ ARMARX_CHECK(pos_diff1 < 2e-5)
+ << "Position is not the same: " << pos << " vs " << propagated.pose.translation();
+ // ARMARX_CHECK((propagated.velocity - vel).norm() < 1e-4) << "Velocity is not the same: " << vel << " vs " << propagated.velocity;
+ // ARMARX_CHECK(propagated.pose.asSO3().lminus(rot).coeffs().norm() < 1e-2) << "Rotation is not the same: " << rot.rotation() << " vs " << propagated.pose.rotation();
+ // state.velocity = vel;
+ state.pose = manif::SE3<T>(pos, rot);
+ states.push_back(propagated);
+
+ tangent = state.pose.log();
+ ARMARX_CHECK((state.pose.translation() - pos).norm() < 1e-6);
+ // ARMARX_CHECK(std::abs(tangent.coeffs()(5) - yaw) < 1e-2);
+
+ // add noise
+ control.velocity.coeffs().segment(0, 3) += acc_noise_std * Vector::Random();
+ control.velocity.coeffs().segment(3, 3) += om_noise_std * Vector::Random();
+ omegas.push_back(control);
+ }
+}
+
+void
+simulate_observation(const std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ObsT>& observations)
+{
+ for (const auto& state : states)
+ {
+ SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
+ SystemModelT::ObsT true_obs;
+ true_obs = state.pose.log().coeffs();
+ // true_obs.segment(0, 3) = state.pose.translation();
+ // true_obs.segment(3, 3) = state.pose.asSO3().log().coeffs();
+ ARMARX_CHECK((obs - true_obs).norm() < std::numeric_limits<T>::epsilon());
+ observations.push_back(obs + obs_noise_std * SystemModelT::ObsT::Random());
+ }
+}
+
+
+BOOST_AUTO_TEST_CASE(se3KalmanFilterTest)
+{
+ srand(time(NULL));
+
+ test_retract();
+ // test_se3();
+
+ std::vector<SystemModelT::StateT> states;
+ std::vector<SystemModelT::ControlT> omegas;
+ std::vector<SystemModelT::ObsT> observations;
+ simulate_trajectory(states, omegas);
+ simulate_observation(states, observations);
+
+ ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size()
+ << " Num Obs: " << observations.size();
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<3, 3>(0, 0) *= acc_noise_std * acc_noise_std;
+ Q.block<3, 3>(3, 3) *= om_noise_std * om_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity() * obs_noise_std * obs_noise_std;
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<3, 3>(3, 3) *= initial_offset_angle * initial_offset_angle;
+ P0.block<3, 3>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
+ // P0.block<3, 3>(6, 6) *= 0.0;
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
+ UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * 1e-3;
+
+ SystemModelT::StateT state0;
+ manif::SO3<T> rot(0, 0, initial_offset_angle);
+ state0.pose = states.at(0).pose.lplus(manif::SE3<T>(initial_offet_pos, rot).log());
+ // state0.velocity = states.at(0).velocity;
+ UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
+
+ std::vector<SystemModelT::StateT> ukf_states;
+ std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
+ ukf_states.push_back(state0);
+ ukf_Ps.push_back(P0);
+
+ std::vector<T> x_true, y_true, z_true, x_obs, y_obs, z_obs, x_ukf, y_ukf, z_ukf;
+ std::vector<T> vx_true, vy_true, vz_true, vx_ukf, vy_ukf, vz_ukf;
+ std::vector<T> a_true, b_true, c_true, a_obs, b_obs, c_obs, a_ukf, b_ukf, c_ukf;
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ // propagate
+ TIMING_START(LOOP);
+ TIMING_START(PROPAGATION);
+ ukf.propagation(omegas.at(i - 1), dt);
+ TIMING_END(PROPAGATION);
+ if ((i - 1) % 100 == 0)
+ {
+ TIMING_START(UPDATE);
+ ukf.update(observations.at(i));
+ TIMING_END(UPDATE);
+ TIMING_START(REST);
+ const SystemModelT::StateT& current_state = ukf.getCurrentState();
+ // ARMARX_INFO << "Velocity Diff: " << (states.at(i).velocity - current_state.velocity).norm();
+ ARMARX_INFO << "Pose Diff: "
+ << (states.at(i).pose - current_state.pose).coeffs().norm();
+ // ARMARX_INFO << "Vel: " << current_state.velocity - states.at(i).velocity;
+ ARMARX_INFO << "Max Cov " << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff();
+ ARMARX_INFO
+ << "Diag: "
+ << (ukf.getCurrentStateCovariance() - Eigen::Matrix<T, 6, 6>::Identity()).norm();
+ // ARMARX_CHECK((states.at(i).position - current_state.position).norm() < 1);
+
+ x_true.push_back(states.at(i).pose.log().coeffs()(0));
+ y_true.push_back(states.at(i).pose.log().coeffs()(1));
+ z_true.push_back(states.at(i).pose.log().coeffs()(2));
+
+ x_obs.push_back(observations.at(i)(0));
+ y_obs.push_back(observations.at(i)(1));
+ z_obs.push_back(observations.at(i)(2));
+
+ x_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(0));
+ y_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(1));
+ z_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(2));
+
+ // vx_true.push_back(states.at(i).velocity(0, 0));
+ // vy_true.push_back(states.at(i).velocity(1, 0));
+ // vz_true.push_back(states.at(i).velocity(2, 0));
+ //
+ // vx_ukf.push_back(ukf.getCurrentState().velocity(0, 0));
+ // vy_ukf.push_back(ukf.getCurrentState().velocity(1, 0));
+ // vz_ukf.push_back(ukf.getCurrentState().velocity(2, 0));
+
+ a_true.push_back(states.at(i).pose.log().coeffs()(3));
+ b_true.push_back(states.at(i).pose.log().coeffs()(4));
+ c_true.push_back(states.at(i).pose.log().coeffs()(5));
+
+ a_obs.push_back(observations.at(i)(3, 0));
+ b_obs.push_back(observations.at(i)(4, 0));
+ c_obs.push_back(observations.at(i)(5, 0));
+
+ a_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(3));
+ b_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(4));
+ c_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(5));
+ TIMING_END(REST);
+ }
+
+ ukf_states.push_back(ukf.getCurrentState());
+ ukf_Ps.push_back(ukf.getCurrentStateCovariance());
+
+ TIMING_END(LOOP);
+ }
+
+ sciplot::Plot3D position_plot;
+ sciplot::Plot2D pos_plot;
+
+ position_plot.drawCurve(x_true, y_true, z_true).label("True").lineWidth(1);
+ position_plot.drawCurve(x_obs, y_obs, z_obs).label("Obs").lineWidth(1);
+ position_plot.drawCurve(x_ukf, y_ukf, z_ukf).label("UKF").lineWidth(1);
+
+ pos_plot.drawCurve(x_true, y_true).label("True");
+ pos_plot.drawCurve(x_obs, y_obs).label("Obs");
+ pos_plot.drawCurve(x_ukf, y_ukf).label("UKF");
+
+ // sciplot::Plot3D vel_plot;
+ //
+ // vel_plot.drawCurve(vx_true, vy_true, vz_true).label("True").lineWidth(1);
+ // vel_plot.drawCurve(vx_ukf, vy_ukf, vz_ukf).label("UKF").lineWidth(1);
+
+
+ sciplot::Plot3D orientation_plot;
+
+ orientation_plot.drawCurve(a_true, b_true, c_true).label("True").lineWidth(1);
+ orientation_plot.drawCurve(a_obs, b_obs, c_obs).label("Obs").lineWidth(1);
+ orientation_plot.drawCurve(a_ukf, b_ukf, c_ukf).label("UKF").lineWidth(1);
+
+ // Create figure to hold plot
+ sciplot::Figure fig = {{pos_plot}, {position_plot}, {orientation_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(600, 600);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("kalman_output.pdf");
+}
diff --git a/source/armarx/navigation/human/test/human_tracker_test.cpp b/source/armarx/navigation/human/test/human_tracker_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..322021491589c466cccc1981fe946819c4caf812
--- /dev/null
+++ b/source/armarx/navigation/human/test/human_tracker_test.cpp
@@ -0,0 +1,247 @@
+/**
+ * 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 Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <iostream>
+
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/interface/core/BasicVectorTypes.h>
+
+#include <VisionX/libraries/armem_human/types.h>
+
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/HumanFilter.h>
+#include <armarx/navigation/human/HumanTracker.h>
+#include <armarx/navigation/human/types.h>
+
+// test includes
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+
+using armarx::armem::human::HumanPose;
+using armarx::armem::human::PoseKeypoint;
+using armarx::navigation::core::Pose2D;
+using CamMm = armarx::navigation::human::HumanTracker::CameraMeasurement;
+using LaserMm = armarx::navigation::human::HumanTracker::LaserMeasurement;
+using Eigen::Vector2f;
+using Ellipsoid = armarx::armem::vision::Ellipsoid;
+using Circle = armarx::armem::vision::Circle;
+
+namespace armarx::navigation::human
+{
+ Eigen::Quaternionf
+ quatFromEuler(float roll, float pitch, float yaw)
+ {
+ return Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) *
+ Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
+ Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ());
+ }
+
+ /**
+ * @brief generateCamMeasurements generates a series of linear camera measurements of a moving
+ * human's head. It will start at startPosition and linearly move to endPosition over the course of
+ * the given timespan, giving a vector with the given amount of steps inbetween.
+ *
+ * @param startPosition the 3D position of the human head at the first measurement
+ * @param endPosition the 3D position of the human head at the last measurement
+ * @param timespanMs the timespan in milliseconds over which the simulated human will move from start to end
+ * @param steps how many measurements will be made (incl. start and end measurement)
+ * @return a vector of <steps> measurements spread equally with regard to position and timespan
+ */
+ std::vector<CamMm>
+ generateCamMeasurements(const Eigen::Vector3f& startPosition,
+ const Eigen::Vector3f& endPosition,
+ const Eigen::Quaternionf& orientation,
+ const int timespanMs,
+ const int steps)
+ {
+ const double timestepMs = static_cast<double>(timespanMs) / steps;
+ const Eigen::Vector3f posDelta = (endPosition - startPosition) / steps;
+
+ std::vector<CamMm> measurements = std::vector<CamMm>();
+ for (int i = 0; i <= steps; i++)
+ {
+ const DateTime t = DateTime(Duration::MilliSeconds(i * timestepMs));
+
+ const Eigen::Vector3f dummyPos = Eigen::Vector3f::Zero();
+ const FramedPosition cameraDummyPos = FramedPosition(dummyPos, "", "");
+
+ const Eigen::Vector3f newPos = startPosition + i * posDelta;
+ const FramedPosition headPosition = FramedPosition(newPos, "", "");
+ const FramedOrientation headOrientation = FramedOrientation(orientation, "", "");
+ const PoseKeypoint head = PoseKeypoint{.label = "HEAD",
+ .confidence = 0.95,
+ .positionCamera = cameraDummyPos,
+ .positionGlobal = headPosition,
+ .orientationGlobal = headOrientation};
+ const HumanPose pose = {.poseModelId = "posemodelid", .keypoints = {{"HEAD", head}}};
+ const std::vector<armem::human::HumanPose> humanPoses = {pose};
+ const CamMm camMm = {t, humanPoses};
+ measurements.emplace_back(camMm);
+ }
+ return measurements;
+ }
+
+ LaserMm
+ generateLaserMeasurement(const Eigen::Vector2f& pos, const DateTime& tLaser)
+ {
+ using Point = Eigen::Vector2f;
+
+ // These are convex
+ const std::vector<Point> relOffsets = {
+ Point(0.01, 0.003), Point(-0.02, 0.007), Point(-0.01, -0.01), Point(0.015, 0.009)};
+
+ const Eigen::Vector2f leftFootPos = pos + Eigen::Vector2f(0, 0.1);
+ const Eigen::Vector2f rightFootPos = pos + Eigen::Vector2f(0, -0.1);
+
+ std::vector<Point> leftFootPoints = std::vector<Point>();
+ std::vector<Point> rightFootPoints = std::vector<Point>();
+ int i = 0;
+ for (const Point& offset : relOffsets)
+ {
+ leftFootPoints.emplace_back(leftFootPos + offset);
+ rightFootPoints.emplace_back(rightFootPos + offset);
+ i++;
+ }
+ std::vector<Point> leftFootHull = std::vector<Point>(leftFootPoints);
+ std::vector<Point> rightFootHull = std::vector<Point>(rightFootPoints);
+
+ // Add one more non-convex point to the cluster points
+ leftFootPoints.emplace_back(leftFootPos + Point(0.002, 0.001));
+ rightFootPoints.emplace_back(rightFootPos + Point(0.002, 0.003));
+
+ using Pose = Eigen::Isometry3f;
+ Pose leftFootPose = Pose::Identity();
+ leftFootPose.translation() = Eigen::Vector3f(leftFootPos.x(), leftFootPos.y(), 0);
+ // leftFootPose.linear() = Eigen::Rotation2Df(orientation).toRotationMatrix();
+
+ Pose rightFootPose = Pose::Identity();
+ rightFootPose.translation() = Eigen::Vector3f(rightFootPos.x(), rightFootPos.y(), 0);
+ ;
+ // rightFootPose.linear() = Eigen::Rotation2Df(orientation).toRotationMatrix();
+
+ const Ellipsoid leftFootEllipse = {.pose = leftFootPose, .radii = Point(0.05, 0.04)};
+ const Ellipsoid rightFootEllipse = {.pose = rightFootPose, .radii = Point(0.05, 0.04)};
+
+ const Circle leftFootCircle = {.center = leftFootPos, .radius = 0.1};
+ const Circle rightFootCircle = {.center = rightFootPos, .radius = 0.1};
+
+ const Cluster leftFootCluster = {
+ .convexHull = leftFootHull,
+ .circle = leftFootCircle,
+ .ellipsoid = leftFootEllipse,
+ .chain = leftFootPoints, //don't use me, probably initialized wrong
+ .points = leftFootPoints};
+ const Cluster rightFootCluster = {
+ .convexHull = rightFootHull,
+ .circle = rightFootCircle,
+ .ellipsoid = rightFootEllipse,
+ .chain = rightFootPoints, //don't use me, probably initialized wrong
+ .points = rightFootPoints};
+
+ const std::vector<Cluster> clusters =
+ std::vector<Cluster>{leftFootCluster, rightFootCluster};
+
+ const LaserMm laserMm = {.detectionTime = tLaser, .clusters = clusters};
+ return laserMm;
+ }
+
+ BOOST_AUTO_TEST_CASE(testLaserTrackingTwoFeetCluster)
+ {
+ HumanTracker tracker = HumanTracker();
+
+ // PARAMETERS
+
+ const Eigen::Vector3f initialPosition = Eigen::Vector3f(0, 0, 2) * 1000;
+ const float orientation = M_PI_2;
+ const Eigen::Quaternionf orientationQuat = quatFromEuler(0, 0, orientation);
+
+ const std::int64_t timestepMs = 100;
+ const Eigen::Vector3f movementSpeedMmPerSec = Eigen::Vector3f(1, 0, 0) * 1000;
+ const int cameraSteps = 100;
+
+ // CONVERSIONS
+ const float timespanMs = timestepMs * cameraSteps;
+ const Eigen::Vector3f endPosition =
+ initialPosition + movementSpeedMmPerSec * timespanMs / 1000;
+ const Eigen::Vector3f posDelta = (endPosition - initialPosition) / cameraSteps;
+
+ // CAMERA MEASUREMENTS
+
+ const std::vector<CamMm> camMeasurements = generateCamMeasurements(
+ initialPosition, endPosition, orientationQuat, timespanMs, cameraSteps);
+
+ for (const CamMm& measurement : camMeasurements)
+ {
+ tracker.update(measurement);
+
+ int i = 0;
+ for (const auto& trackedHuman : tracker.getTrackedHumans())
+ {
+ const Eigen::Vector2f tPos = trackedHuman.pose.translation();
+ BOOST_TEST_MESSAGE("tracker now thinks human "
+ << i << " is at x=(" << tPos.x() << "," << tPos.y()
+ << ") moving with v=(" << trackedHuman.linearVelocity.x() << ","
+ << trackedHuman.linearVelocity.y() << ")");
+ i++;
+ }
+ }
+
+ // LASER MEASUREMENT
+
+ const DateTime tLaser = DateTime(Duration::MilliSeconds(timespanMs + timestepMs));
+ Eigen::Vector2f laserPos = (endPosition + posDelta).segment(0, 2);
+ const LaserMm laserMm = generateLaserMeasurement(laserPos, tLaser);
+
+ tracker.update(laserMm);
+
+ const std::vector<Human> humans = tracker.getTrackedHumans();
+ BOOST_CHECK_EQUAL(humans.size(), 1); // should have detected the one human through laser
+ const Human h = humans.at(0);
+ BOOST_CHECK(h.pose.translation().x() - laserPos.x() < 0.05 * 1000);
+ BOOST_CHECK(h.pose.translation().y() - laserPos.y() < 0.05 * 1000);
+ BOOST_TEST_MESSAGE("final position: x=(" << h.pose.translation().x() << ","
+ << h.pose.translation().y() << ")");
+ BOOST_TEST_MESSAGE("final velocity: v=(" << h.linearVelocity.x() << ","
+ << h.linearVelocity.y() << ")");
+ BOOST_CHECK(h.linearVelocity.x() - movementSpeedMmPerSec.x() < 0.05 * 1000);
+ BOOST_CHECK(h.linearVelocity.y() - movementSpeedMmPerSec.y() < 0.05 * 1000);
+ }
+
+
+ BOOST_AUTO_TEST_CASE(testRemoveOldHumans)
+ {
+ HumanTracker tracker = HumanTracker();
+ auto zero = Eigen::Vector3f(0.f, 0.f, 0.f);
+ auto measurements =
+ generateCamMeasurements(zero, zero, Eigen::Quaternionf::Identity(), 0, 1);
+ tracker.update(measurements.at(0));
+
+ BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 1);
+
+ CamMm late = {.detectionTime = DateTime(Duration::MilliSeconds(1000))};
+ tracker.update(late);
+
+ BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 0);
+ }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/kalman_filter_test.cpp b/source/armarx/navigation/human/test/kalman_filter_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..04cf7c69a1d530d61d172871ec1eca58a03a8729
--- /dev/null
+++ b/source/armarx/navigation/human/test/kalman_filter_test.cpp
@@ -0,0 +1,112 @@
+/**
+ * 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 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
+ */
+
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/HumanFilter.h>
+#include <armarx/navigation/human/types.h>
+
+// test includes
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+
+using armarx::navigation::core::Pose2D;
+using Eigen::Vector2f;
+
+namespace armarx::navigation::human
+{
+ Pose2D
+ generatePose(double x, double y, double yaw)
+ {
+ Pose2D pose = Pose2D::Identity();
+ pose.translation() = Eigen::Vector2f(x, y);
+ pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
+
+ return pose;
+ }
+
+ DateTime
+ generateTime(double time)
+ {
+ return DateTime(time);
+ }
+
+ double
+ extractRotation(const Pose2D& pose)
+ {
+ return Eigen::Rotation2Df(pose.linear()).angle();
+ }
+
+
+ bool
+ doublesEqual(const double d1, const double d2, const double relErr)
+ {
+ if (d1 == 0)
+ {
+ return d2 == 0;
+ }
+ return std::abs(d1 - d2) / d1 <= relErr;
+ }
+
+ bool
+ posesEqual(const Pose2D& p1, const Pose2D& p2, const double relErr)
+ {
+ return doublesEqual(p1.translation().x(), p2.translation().x(), relErr) &&
+ doublesEqual(p1.translation().y(), p2.translation().y(), relErr) &&
+ doublesEqual(extractRotation(p1), extractRotation(p2), relErr);
+ }
+
+ bool
+ vectorsEqual(const Vector2f& v1, const Vector2f v2, const double relErr)
+ {
+ return doublesEqual(v1.x(), v2.x(), relErr) && doublesEqual(v1.y(), v2.y(), relErr);
+ }
+
+ bool
+ timesEqual(const DateTime& t1, const DateTime& t2, const double relErr)
+ {
+ return doublesEqual(t1.toMilliSecondsSinceEpoch(), t2.toMilliSecondsSinceEpoch(), relErr);
+ }
+
+ bool
+ humansEqual(const Human& h1, const Human& h2, const double relErr)
+ {
+ return posesEqual(h1.pose, h2.pose, relErr) &&
+ vectorsEqual(h1.linearVelocity, h2.linearVelocity, relErr) &&
+ timesEqual(h1.detectionTime, h2.detectionTime, relErr);
+ }
+
+
+ BOOST_AUTO_TEST_CASE(testInitialization)
+ {
+ HumanFilter filter(generatePose(3, 2, 1), generateTime(4));
+
+ Human human{.pose = generatePose(3, 2, 1),
+ .linearVelocity = Vector2f::Zero(),
+ .detectionTime = generateTime(4)};
+
+ BOOST_TEST(humansEqual(filter.get(), human, 0.001),
+ "filter should not have changed humans initial pose properties");
+ }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/manifKalmanTest.cpp b/source/armarx/navigation/human/test/manifKalmanTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4940ab7cd36227965209ac837a676b265816f06a
--- /dev/null
+++ b/source/armarx/navigation/human/test/manifKalmanTest.cpp
@@ -0,0 +1,462 @@
+/**
+ * \file se2_localization_ukfm.cpp
+ *
+ * Created on: Dec 10, 2018
+ * \author: artivis
+ *
+ * ---------------------------------------------------------
+ * This file is:
+ * (c) 2021 Jeremie Deray
+ *
+ * This file is part of `manif`, a C++ template-only library
+ * for Lie theory targeted at estimation for robotics.
+ * Manif is:
+ * (c) 2018 Jeremie Deray @ IRI-UPC, Barcelona
+ * ---------------------------------------------------------
+ *
+ * ---------------------------------------------------------
+ * Demonstration example:
+ *
+ * 2D Robot localization based on fixed beacons.
+ *
+ * See se3_localization.cpp for the 3D equivalent.
+ * See se3_sam.cpp for a more advanced example performing smoothing and mapping.
+ * ---------------------------------------------------------
+ *
+ * This demo showcases an application of an Unscented Kalman Filter on Manifold,
+ * based on the paper
+ * 'A Code for Unscented Kalman Filtering on Manifolds (UKF-M)'
+ * [https://arxiv.org/pdf/2002.00878.pdf], M. Brossard, A. Barrau and S. Bonnabel.
+ *
+ * The following is an abstract of the example hereafter.
+ * Please consult the aforemention paper for better UKF-M reference
+ * and the paper Sola-18, [https://arxiv.org/abs/1812.01537] for general
+ * Lie group reference.
+ *
+ *
+ * We consider a robot in the plane surrounded by a small
+ * number of punctual landmarks or _beacons_.
+ * The robot receives control actions in the form of axial
+ * and angular velocities, and is able to measure the location
+ * of the beacons w.r.t its own reference frame.
+ *
+ * The robot pose X is in SE(2) and the beacon positions b_k in R^2,
+ *
+ * | cos th -sin th x |
+ * X = | sin th cos th y | // position and orientation
+ * | 0 0 1 |
+ *
+ * b_k = (bx_k, by_k) // lmk coordinates in world frame
+ *
+ * The control signal u is a twist in se(2) comprising longitudinal
+ * velocity v and angular velocity w, with no lateral velocity
+ * component, integrated over the sampling time dt.
+ *
+ * u = (v*dt, 0, w*dt)
+ *
+ * The control is corrupted by additive Gaussian noise u_noise,
+ * with covariance
+ *
+ * Q = diagonal(sigma_v^2, sigma_s^2, sigma_w^2).
+ *
+ * This noise accounts for possible lateral slippage u_s
+ * through a non-zero value of sigma_s,
+ *
+ * At the arrival of a control u, the robot pose is updated
+ * with X <-- X * Exp(u) = X + u.
+ *
+ * Landmark measurements are of the range and bearing type,
+ * though they are put in Cartesian form for simplicity.
+ * Their noise n is zero mean Gaussian, and is specified
+ * with a covariances matrix R.
+ * We notice the rigid motion action y = h(X,b) = X^-1 * b
+ * (see appendix C),
+ *
+ * y_k = (brx_k, bry_k) // lmk coordinates in robot frame
+ *
+ * We consider the beacons b_k situated at known positions.
+ * We define the pose to estimate as X in SE(2).
+ * The estimation error dx and its covariance P are expressed
+ * in the tangent space at X.
+ *
+ * All these variables are summarized again as follows
+ *
+ * X : robot pose, SE(2)
+ * u : robot control, (v*dt ; 0 ; w*dt) in se(2)
+ * Q : control perturbation covariance
+ * b_k : k-th landmark position, R^2
+ * y : Cartesian landmark measurement in robot frame, R^2
+ * R : covariance of the measurement noise
+ *
+ * The motion and measurement models are
+ *
+ * X_(t+1) = f(X_t, u) = X_t * Exp ( w ) // motion equation
+ * y_k = h(X, b_k) = X^-1 * b_k // measurement equation
+ *
+ * The algorithm below comprises first a simulator to
+ * produce measurements, then uses these measurements
+ * to estimate the state, using a Lie-based error-state Kalman filter.
+ *
+ * This file has plain code with only one main() function.
+ * There are no function calls other than those involving `manif`.
+ *
+ * Printing simulated state and estimated state together
+ * with an unfiltered state (i.e. without Kalman corrections)
+ * allows for evaluating the quality of the estimates.
+ */
+
+#include <iomanip>
+#include <iostream>
+#include <tuple>
+#include <vector>
+
+#include <Eigen/Cholesky>
+
+#include "manif/SE2.h"
+#include <sciplot/sciplot.hpp>
+
+using std::cout;
+using std::endl;
+
+using namespace Eigen;
+
+typedef Array<double, 2, 1> Array2d;
+typedef Array<double, 3, 1> Array3d;
+
+template <typename Scalar>
+struct Weights
+{
+ Weights() = default;
+ ~Weights() = default;
+
+ Weights(const Scalar l, const Scalar alpha)
+ {
+ using std::sqrt;
+
+ const Scalar m = (alpha * alpha - 1) * l;
+ const Scalar ml = m + l;
+
+ sqrt_d_lambda = sqrt(ml);
+ wj = Scalar(1) / (Scalar(2) * (ml));
+ wm = m / (ml);
+ w0 = m / (ml) + Scalar(3) - alpha * alpha;
+ }
+
+ Scalar sqrt_d_lambda;
+ Scalar wj;
+ Scalar wm;
+ Scalar w0;
+};
+
+using Weightsd = Weights<double>;
+
+template <typename Scalar>
+std::tuple<Weights<Scalar>, Weights<Scalar>, Weights<Scalar>>
+compute_sigma_weights(const Scalar state_size,
+ const Scalar propagation_noise_size,
+ const Scalar alpha_0,
+ const Scalar alpha_1,
+ const Scalar alpha_2)
+{
+ assert(state_size > 0);
+ assert(propagation_noise_size > 0);
+ assert(alpha_0 >= 1e-3 && alpha_0 <= 1);
+ assert(alpha_1 >= 1e-3 && alpha_1 <= 1);
+ assert(alpha_2 >= 1e-3 && alpha_2 <= 1);
+
+ return std::make_tuple(Weights<Scalar>(state_size, alpha_0),
+ Weights<Scalar>(propagation_noise_size, alpha_1),
+ Weights<Scalar>(state_size, alpha_2));
+}
+
+int
+main()
+{
+ std::srand((unsigned int)time(0));
+
+ // START CONFIGURATION
+ //
+ //
+ const int NUMBER_OF_LMKS_TO_MEASURE = 3;
+ constexpr int DoF = manif::SE2d::DoF;
+ constexpr int SystemNoiseSize = manif::SE2d::DoF;
+ // Measurement Dim
+ constexpr int Rp = 2;
+
+ // Define the robot pose element and its covariance
+ manif::SE2d X = manif::SE2d::Identity(), X_simulation = manif::SE2d::Identity(),
+ X_unfiltered = manif::SE2d::Identity();
+ Matrix3d P = Matrix3d::Identity() * 1e-6;
+
+ // Define a control vector and its noise and covariance
+ manif::SE2Tangentd u_simu, u_est, u_unfilt;
+ Vector3d u_nom, u_noisy, u_noise;
+ Array3d u_sigmas;
+ Matrix3d U, Uchol;
+
+ u_nom << 0.1, 0.0, 0.05;
+ u_sigmas << 0.1, 0.1, 0.1;
+ U = (u_sigmas * u_sigmas).matrix().asDiagonal();
+ Uchol = U.llt().matrixL();
+
+ // Define three landmarks in R^2
+ Eigen::Vector2d b;
+ const std::vector<Eigen::Vector2d> landmarks{
+ Eigen::Vector2d(2.0, 0.0), Eigen::Vector2d(2.0, 1.0), Eigen::Vector2d(2.0, -1.0)};
+
+ // Define the beacon's measurements
+ Vector2d y, y_bar, y_noise;
+ Matrix<double, Rp, 2 * DoF> yj;
+ Array2d y_sigmas;
+ Matrix2d R;
+ std::vector<Vector2d> measurements(landmarks.size());
+
+ y_sigmas << 0.01, 0.01;
+ R = (y_sigmas * y_sigmas).matrix().asDiagonal();
+
+
+ // Declare UFK variables
+ Array3d alpha;
+ alpha << 1e-3, 1e-3, 1e-3;
+
+ Weightsd w_d, w_q, w_u;
+ std::tie(w_d, w_q, w_u) = compute_sigma_weights<double>(DoF, Rp, alpha(0), alpha(1), alpha(2));
+
+ // Declare some temporaries
+
+ manif::SE2d X_new;
+ Matrix3d P_new;
+ manif::SE2d s_j_p, s_j_m;
+ Vector3d xi_mean;
+ Vector3d w_p, w_m;
+
+ Matrix2d P_yy;
+ Matrix<double, DoF, 2 * DoF> xij;
+ Matrix<double, DoF, 2> P_xiy;
+
+ Vector2d e, z; // expectation, innovation
+ Matrix<double, 3, 2> K; // Kalman gain
+ manif::SE2Tangentd dx; // optimal update step, or error-state
+
+ Matrix<double, DoF, DoF> xis;
+ Matrix<double, DoF, DoF * 2> xis_new;
+ Matrix<double, DoF, SystemNoiseSize * 2> xis_new2;
+
+ //
+ //
+ // CONFIGURATION DONE
+
+
+ // DEBUG
+ cout << std::fixed << std::setprecision(4) << std::showpos << endl;
+ cout << "X STATE : X Y THETA" << endl;
+ cout << "----------------------------------" << endl;
+ cout << "X initial : " << X_simulation.log().coeffs().transpose() << endl;
+ cout << "----------------------------------" << endl;
+ // END DEBUG
+
+
+ std::vector<double> X_simulated_x, X_simulated_y, X_simulated_theta;
+ std::vector<double> X_estimated_x, X_estimated_y, X_estimated_theta;
+ std::vector<double> X_unfiltered_x, X_unfiltered_y, X_unfiltered_theta;
+
+ // START TEMPORAL LOOP
+ //
+ //
+
+ // Make 10 steps. Measure up to three landmarks each time.
+ for (int t = 0; t < 10; t++)
+ {
+ //// I. Simulation ###############################################################################
+
+ /// simulate noise
+ u_noise = u_sigmas * Array3d::Random(); // control noise
+ u_noisy = u_nom + u_noise; // noisy control
+
+ u_simu = u_nom;
+ u_est = u_noisy;
+ u_unfilt = u_noisy;
+
+ /// first we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+ X_simulation = X_simulation + u_simu; // overloaded X.rplus(u) = X * exp(u)
+
+ /// then we measure all landmarks - - - - - - - - - - - - - - - - - - - -
+ for (std::size_t i = 0; i < landmarks.size(); i++)
+ {
+ b = landmarks[i]; // lmk coordinates in world frame
+
+ /// simulate noise
+ y_noise = y_sigmas * Array2d::Random(); // measurement noise
+
+ y = X_simulation.inverse().act(b); // landmark measurement, before adding noise
+ y = y + y_noise; // landmark measurement, noisy
+ measurements[i] = y; // store for the estimator just below
+ }
+
+
+ //// II. Estimation ###############################################################################
+
+ /// First we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+
+ X_new = X + u_est; // X * exp(u)
+
+ // set sigma points
+ xis = w_d.sqrt_d_lambda * P.llt().matrixL().toDenseMatrix();
+
+ // sigma points on manifold
+ for (int i = 0; i < DoF; ++i)
+ {
+ s_j_p = X + manif::SE2Tangentd(xis.col(i));
+ s_j_m = X + manif::SE2Tangentd(-xis.col(i));
+
+ xis_new.col(i) = (X_new.lminus(s_j_p + u_est)).coeffs();
+ xis_new.col(i + DoF) = (X_new.lminus(s_j_m + u_est)).coeffs();
+ }
+
+ // compute covariance
+ xi_mean = w_d.wj * xis_new.rowwise().sum();
+ xis_new.colwise() -= xi_mean;
+
+ P_new = w_d.wj * xis_new * xis_new.transpose() + w_d.w0 * xi_mean * xi_mean.transpose();
+
+ // sigma points on manifold
+ for (int i = 0; i < SystemNoiseSize; ++i)
+ {
+ w_p = w_q.sqrt_d_lambda * Uchol.col(i);
+ w_m = -w_q.sqrt_d_lambda * Uchol.col(i);
+
+ xis_new2.col(i) = (X_new.lminus(X + (u_est + w_p))).coeffs();
+ xis_new2.col(i + SystemNoiseSize) = (X_new.lminus(X + (u_est + w_m))).coeffs();
+ }
+
+ xi_mean = w_q.wj * xis_new2.rowwise().sum();
+ xis_new2.colwise() -= xi_mean;
+
+ U = w_q.wj * xis_new2 * xis_new2.transpose() + w_q.w0 * xi_mean * xi_mean.transpose();
+
+ P = P_new + U;
+
+ X = X_new;
+
+ /// Then we correct using the measurements of each lmk - - - - - - - - -
+ for (int i = 0; i < NUMBER_OF_LMKS_TO_MEASURE; i++)
+ {
+ // landmark
+ b = landmarks[i]; // lmk coordinates in world frame
+
+ // measurement
+ y = measurements[i]; // lmk measurement, noisy
+
+ // expectation
+ e = X.inverse().act(b);
+
+ // set sigma points
+ xis = w_u.sqrt_d_lambda * P.llt().matrixL().toDenseMatrix();
+
+ // compute measurement sigma points
+ for (int d = 0; d < DoF; ++d)
+ {
+ s_j_p = X + manif::SE2Tangentd(xis.col(d));
+ s_j_m = X + manif::SE2Tangentd(-xis.col(d));
+
+ yj.col(d) = s_j_p.inverse().act(b);
+ yj.col(d + DoF) = s_j_m.inverse().act(b);
+ }
+
+ // measurement mean
+ y_bar = w_u.wm * e + w_u.wj * yj.rowwise().sum();
+
+ yj.colwise() -= y_bar;
+ e -= y_bar;
+
+ // compute covariance and cross covariance matrices
+ P_yy = w_u.w0 * e * e.transpose() + w_u.wj * yj * yj.transpose() + R;
+
+ xij << xis, -xis;
+ P_xiy = w_u.wj * xij * yj.transpose();
+
+ // Kalman gain
+ K = P_yy.colPivHouseholderQr().solve(P_xiy.transpose()).transpose();
+
+ // innovation
+ z = y - y_bar;
+
+ // Correction step
+ dx = K * z; // dx is in the tangent space at X
+
+ // Update
+ X = X + dx; // overloaded X.rplus(dx) = X * exp(dx)
+ P = P - K * P_yy * K.transpose();
+ }
+
+
+ //// III. Unfiltered ##############################################################################
+
+ // move also an unfiltered version for comparison purposes
+ X_unfiltered = X_unfiltered + u_unfilt;
+
+
+ //// IV. Results ##############################################################################
+
+ // DEBUG
+ cout << "X simulated : " << X_simulation.log().coeffs().transpose() << "\n";
+ cout << "X estimated : " << X.log().coeffs().transpose() << "\n";
+ cout << "X unfilterd : " << X_unfiltered.log().coeffs().transpose() << "\n";
+ cout << "----------------------------------" << endl;
+
+ const auto& X_simu = X_simulation.log().coeffs().transpose();
+ const auto& X_est = X.log().coeffs().transpose();
+ const auto& X_unf = X_unfiltered.log().coeffs().transpose();
+ X_simulated_x.push_back(X_simu(0));
+ X_simulated_y.push_back(X_simu(1));
+ X_simulated_theta.push_back(X_simu(2));
+ X_estimated_x.push_back(X_est(0));
+ X_estimated_y.push_back(X_est(1));
+ X_estimated_theta.push_back(X_est(2));
+ X_unfiltered_x.push_back(X_unf(0));
+ X_unfiltered_y.push_back(X_unf(1));
+ X_unfiltered_theta.push_back(X_unf(2));
+ // END DEBUG
+ }
+
+ sciplot::Plot2D pos_plot;
+ pos_plot.xlabel("x");
+ pos_plot.ylabel("y");
+ pos_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ pos_plot.xrange("*", "*").yrange("*", "*");
+
+ pos_plot.drawCurve(X_simulated_x, X_simulated_y).label("Simulated");
+ pos_plot.drawCurve(X_estimated_x, X_estimated_y).label("Estimated");
+ pos_plot.drawCurve(X_unfiltered_x, X_unfiltered_y).label("Unfiltered");
+
+
+ sciplot::Plot2D orientation_plot;
+ orientation_plot.xlabel("t");
+ orientation_plot.ylabel("rot");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange("*", "*").yrange("*", "*");
+
+ sciplot::Vec x = sciplot::linspace(0.0, 1.0, X_estimated_theta.size());
+ orientation_plot.drawCurve(x, X_simulated_theta).label("Simulated");
+ orientation_plot.drawCurve(x, X_estimated_theta).label("Estimated");
+ orientation_plot.drawCurve(x, X_unfiltered_theta).label("Unfiltered");
+
+ // Create figure to hold plot
+ sciplot::Figure fig = {{pos_plot, orientation_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(1000, 500);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("manif_kalman_output.pdf");
+
+
+ //
+ //
+ // END OF TEMPORAL LOOP. DONE.
+
+ return 0;
+}
diff --git a/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp b/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c83f4db5b4fedc49a6af0077229a075ea9e7cd25
--- /dev/null
+++ b/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp
@@ -0,0 +1,296 @@
+/**
+ * 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 )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/time.h>
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <manif/SO2.h>
+#include <sciplot/sciplot.hpp>
+
+namespace armarx::navigation::human
+{
+ using T = double; //TODO double or float?
+ using Vector = Eigen::Matrix<T, 2, 1>;
+ using SystemModelT = kalman_filter::SystemModelSE2<T>;
+
+ constexpr T RAD = 10;
+ constexpr long num_timesteps = 3000;
+ constexpr T max_time = 1;
+ constexpr T dt = max_time / num_timesteps;
+ constexpr T c = (1 / max_time) * 2 * M_PI; // rotation per time
+
+ constexpr T rot_noise_std = 0.1;
+ constexpr T pos_noise_std = 1;
+ constexpr T obs_rot_noise_std = 0.1;
+ constexpr T obs_pos_noise_std = 1;
+ constexpr T initial_offset_angle = 0.1 * 10 * M_PI / 180;
+ const Vector initial_offet_pos = 0.1 * RAD * Vector(1, 0.5);
+
+
+ void
+ simulate_trajectory(std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ControlT>& omegas)
+ {
+ constexpr T startAngle = 0;
+
+ SystemModelT::StateT state;
+ state.pose = manif::SE2<T>(RAD, 0, startAngle);
+ states.push_back(state);
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T angle = t * c;
+
+ const Vector last_pos = states.at(i - 1).pose.translation();
+ //const Vector pos(angle, std::cos(angle));
+ const Vector pos = RAD * Vector(std::cos(angle), std::sin(angle));
+
+ BOOST_TEST(((last_pos - pos).norm() < 0.1 * RAD),
+ "position differs too much from last step: " << (last_pos - pos).norm());
+
+ Eigen::Vector2d euclidean_velocity = RAD * Eigen::Vector2d{0, c};
+ SystemModelT::ControlT control;
+ //control.velocity.coeffs() << (pos - last_pos) / dt, c;
+ control.velocity.coeffs() << euclidean_velocity, c;
+
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
+ state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+
+ state.pose = manif::SE2<T>(pos.x(), pos.y(), angle + startAngle);
+
+ T pos_diff = (propagated.pose.translation() - state.pose.translation()).norm();
+ BOOST_TEST(pos_diff < 2e-10,
+ "propagated position differs too much from real position: " << pos_diff);
+
+ T rot_diff = propagated.pose.angle() - state.pose.angle();
+ BOOST_TEST(rot_diff < 2e-10,
+ "propagated rotation differs too much from real orientation: " << rot_diff);
+
+ states.push_back(propagated);
+
+
+ // add noise
+ control.velocity.coeffs().segment(0, 2) += pos_noise_std * Vector::Random();
+ control.velocity.coeffs().segment(2, 1) +=
+ rot_noise_std * Eigen::Matrix<T, 1, 1>::Random();
+ omegas.push_back(control);
+ }
+ }
+
+ void
+ simulate_observation(const std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ObsT>& observations)
+ {
+ for (const auto& state : states)
+ {
+ SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
+
+ SystemModelT::ObsT noise = SystemModelT::ObsT::Random();
+ noise.segment(0, 2) *= obs_pos_noise_std;
+ noise.segment(2, 1) *= obs_rot_noise_std;
+
+ BOOST_TEST_MESSAGE("SALT TEST: noise " << noise);
+
+ observations.push_back(obs + noise);
+
+ BOOST_TEST_MESSAGE("SALT TEST: state:\n"
+ << state.pose.x() << "\n"
+ << state.pose.y() << "\n"
+ << state.pose.angle());
+
+ BOOST_TEST_MESSAGE("SALT TEST: state.log:\n" << state.pose.log().coeffs());
+
+ BOOST_TEST_MESSAGE("SALT TEST: noisy obs:\n" << (obs + noise));
+
+ SystemModelT::ObsT diff =
+ (obs + noise) -
+ SystemModelT::ObsT(state.pose.x(), state.pose.y(), state.pose.angle());
+
+ BOOST_TEST_MESSAGE("\nSALT TEST: diff:\n" << diff);
+ }
+ }
+
+
+ BOOST_AUTO_TEST_CASE(se2KalmanFilterTest)
+ {
+ srand(time(NULL));
+
+ std::vector<SystemModelT::StateT> states;
+ std::vector<SystemModelT::ControlT> omegas;
+ std::vector<SystemModelT::ObsT> observations;
+ simulate_trajectory(states, omegas);
+ simulate_observation(states, observations);
+
+ ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size()
+ << " Num Obs: " << observations.size();
+
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<2, 2>(0, 0) *= pos_noise_std * pos_noise_std;
+ Q.block<1, 1>(2, 2) *= rot_noise_std * rot_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<2, 2>(0, 0) *= obs_pos_noise_std * obs_pos_noise_std;
+ R.block<1, 1>(2, 2) *= obs_rot_noise_std * obs_rot_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<2, 2>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
+ P0.block<1, 1>(2, 2) *= initial_offset_angle * initial_offset_angle;
+
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha;
+ alpha(0) = 0.5; //0.8; // state
+ alpha(1) = 0.5; //0.1; // control
+ alpha(2) = 0.5; //0.1; // update
+
+ SystemModelT::StateT state0;
+ const Vector startPos = states.at(0).pose.translation() + initial_offet_pos;
+ state0.pose = manif::SE2<T>{
+ startPos.x(), startPos.y(), states.at(0).pose.angle() + initial_offset_angle};
+
+ UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
+
+ std::vector<SystemModelT::StateT> ukf_states;
+ std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
+ ukf_states.push_back(state0);
+ ukf_Ps.push_back(P0);
+
+ std::vector<T> x_true, y_true, x_obs, y_obs, x_ukf, y_ukf;
+ std::vector<T> a_true, a_obs, a_ukf;
+ std::vector<T> x_ukf_full, y_ukf_full, a_ukf_full;
+ std::vector<T> ukf_pos_var, ukf_rot_var, ukf_var;
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ // propagate
+ TIMING_START(LOOP);
+ TIMING_START(PROPAGATION);
+ ukf.propagation(omegas.at(i - 1), dt);
+ TIMING_END(PROPAGATION);
+ if ((i - 1) % 1 == 0)
+ {
+ TIMING_START(UPDATE);
+ ukf.update(observations.at(i));
+ TIMING_END(UPDATE);
+ TIMING_START(REST);
+
+ BOOST_TEST_MESSAGE("Max Cov "
+ << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+ BOOST_TEST_MESSAGE("Diag: " << (ukf.getCurrentStateCovariance() -
+ Eigen::Matrix<T, 3, 3>::Identity())
+ .norm());
+
+ manif::SE2Tangent<T> obs{observations.at(i)};
+ x_obs.push_back(obs.exp().x());
+ y_obs.push_back(obs.exp().y());
+ a_obs.push_back(obs.exp().angle());
+
+ x_ukf.push_back(ukf.getCurrentState().pose.x());
+ y_ukf.push_back(ukf.getCurrentState().pose.y());
+ a_ukf.push_back(ukf.getCurrentState().pose.angle());
+
+ TIMING_END(REST);
+ }
+ x_true.push_back(states.at(i).pose.x());
+ y_true.push_back(states.at(i).pose.y());
+ a_true.push_back(states.at(i).pose.angle());
+
+ x_ukf_full.push_back(ukf.getCurrentState().pose.x());
+ y_ukf_full.push_back(ukf.getCurrentState().pose.y());
+ a_ukf_full.push_back(ukf.getCurrentState().pose.angle());
+
+ ukf_states.push_back(ukf.getCurrentState());
+ ukf_Ps.push_back(ukf.getCurrentStateCovariance());
+
+ ukf_pos_var.push_back(
+ ukf.getCurrentStateCovariance().block<2, 2>(0, 0).cwiseAbs().maxCoeff());
+ ukf_rot_var.push_back(
+ ukf.getCurrentStateCovariance().block<1, 1>(2, 2).cwiseAbs().maxCoeff());
+ ukf_var.push_back(ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+
+ TIMING_END(LOOP);
+ }
+
+ sciplot::Plot2D pos_plot;
+ pos_plot.xlabel("x");
+ pos_plot.ylabel("y");
+ pos_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ pos_plot.xrange(-RAD * 1.2, RAD * 1.2).yrange(-RAD * 1.2, RAD * 1.2);
+ //pos_plot.xrange(-4, 4).yrange(-4, 4);
+
+ pos_plot.drawCurve(x_true, y_true).label("True");
+ pos_plot.drawCurve(x_obs, y_obs).label("Obs");
+ pos_plot.drawCurveWithPoints(x_ukf, y_ukf).label("UKF");
+ pos_plot.drawCurve(x_ukf_full, y_ukf_full).label("Full UKF");
+
+
+ sciplot::Plot2D orientation_plot;
+ orientation_plot.xlabel("t");
+ orientation_plot.ylabel("rot");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange(-0.1, 1.1).yrange(-4, 4);
+
+ sciplot::Vec x = sciplot::linspace(0.0, 1.0, a_obs.size());
+ sciplot::Vec x_t = sciplot::linspace(0.0, 1.0, a_true.size());
+ orientation_plot.drawCurve(x_t, a_true).label("True").lineWidth(1);
+ orientation_plot.drawCurve(x, a_obs).label("Obs").lineWidth(1);
+ orientation_plot.drawCurve(x, a_ukf).label("UKF").lineWidth(1);
+ orientation_plot.drawCurve(x_t, a_ukf_full).label("Full UKF").lineWidth(1);
+
+
+ sciplot::Plot2D error_plot;
+ error_plot.xlabel("t");
+ error_plot.ylabel("err");
+ error_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ error_plot.xrange(-0.1, 1.1).yrange("*", "*");
+
+ error_plot.drawCurve(x_t, ukf_pos_var).label("Pos variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_rot_var).label("Rot variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_var).label("Total variance").lineWidth(1);
+
+
+ // Create figure to hold plot
+ sciplot::Figure fig = {{pos_plot, orientation_plot}, {error_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(1000, 1000);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("se2_kalman_output.pdf");
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp b/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9693e0f83b60b5b93d01a549e4d8ca1e2d4c0f14
--- /dev/null
+++ b/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp
@@ -0,0 +1,292 @@
+/**
+ * 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 )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/time.h>
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <manif/SO2.h>
+#include <sciplot/sciplot.hpp>
+
+namespace armarx::navigation::human
+{
+ using T = double; //TODO double or float?
+ using Vector = Eigen::Matrix<T, 2, 1>;
+ using SystemModelT = kalman_filter::SystemModelSE2xV<T>;
+
+ constexpr T RAD = 1;
+ constexpr long num_timesteps = 3000;
+ constexpr T max_time = 1;
+ constexpr T dt = max_time / num_timesteps;
+ constexpr T c = (1 / max_time) * 2 * M_PI; // rotation per time
+
+ constexpr T obs_rot_noise_std = 0.01;
+ constexpr T obs_pos_noise_std = 0.01;
+ constexpr T initial_offset_angle = 1 * 10 * M_PI / 180;
+ const Vector initial_offet_pos = 0.8 * RAD * Vector(1, 0.5);
+ const Eigen::Matrix<T, 3, 1> initial_offet_vel =
+ 0.8 * RAD * Eigen::Matrix<T, 3, 1>(1, 0.5, 0.5);
+
+
+ void
+ simulate_trajectory(std::vector<SystemModelT::StateT>& states)
+ {
+ constexpr T startAngle = 0;
+
+ SystemModelT::StateT state;
+ state.pose = manif::SE2<T>(RAD, 0, startAngle);
+ state.velocity = manif::SE2Tangent<T>(0, c, c);
+ states.push_back(state);
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T angle = t * c;
+
+ const Vector last_pos = states.at(i - 1).pose.translation();
+ //const Vector pos(angle, std::cos(angle));
+ const Vector pos = RAD * Vector(std::cos(angle), std::sin(angle));
+
+ BOOST_TEST(((last_pos - pos).norm() < 0.1 * RAD),
+ "position differs too much from last step: " << (last_pos - pos).norm());
+
+ SystemModelT::ControlT control;
+
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
+ state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+
+ state.pose = manif::SE2<T>(pos.x(), pos.y(), angle + startAngle);
+ state.velocity = manif::SE2Tangent<T>(0, c, c);
+
+ T pos_diff = (propagated.pose.translation() - state.pose.translation()).norm();
+ BOOST_TEST(pos_diff < 2e-10,
+ "propagated position differs too much from real position: " << pos_diff);
+
+ T rot_diff = propagated.pose.angle() - state.pose.angle();
+ BOOST_TEST(rot_diff < 2e-10,
+ "propagated rotation differs too much from real orientation: " << rot_diff);
+
+ T vel_diff = (propagated.velocity.coeffs() - state.velocity.coeffs()).norm();
+ BOOST_TEST(vel_diff < 2e-10,
+ "propagated velocity differs too much from real velocity: " << vel_diff);
+
+ states.push_back(state);
+ }
+ }
+
+ void
+ simulate_observation(const std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ObsT>& observations)
+ {
+ for (const auto& state : states)
+ {
+ SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
+
+ SystemModelT::ObsT noise = SystemModelT::ObsT::Random();
+ noise.segment(0, 2) *= obs_pos_noise_std;
+ noise.segment(2, 1) *= obs_rot_noise_std;
+
+ observations.push_back(obs + noise);
+ }
+ }
+
+
+ BOOST_AUTO_TEST_CASE(se2xVkalmanFilterTest)
+ {
+ srand(time(NULL));
+
+ std::vector<SystemModelT::StateT> states;
+ std::vector<SystemModelT::ObsT> observations;
+ simulate_trajectory(states);
+ simulate_observation(states, observations);
+
+ ARMARX_INFO << "Num States: " << states.size() << " Num Obs: " << observations.size();
+
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<2, 2>(0, 0) *= obs_pos_noise_std * obs_pos_noise_std;
+ R.block<1, 1>(2, 2) *= obs_rot_noise_std * obs_rot_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<2, 2>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
+ P0.block<1, 1>(2, 2) *= initial_offset_angle * initial_offset_angle;
+ P0.block<3, 3>(3, 3) *= initial_offet_vel.norm() * initial_offet_vel.norm();
+
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha;
+ alpha(0) = 0.001; // state
+ alpha(1) = 0.001; // control
+ alpha(2) = 0.001; // update
+
+ SystemModelT::StateT state0;
+ const Vector startPos = states.at(0).pose.translation() + initial_offet_pos;
+ state0.pose = manif::SE2<T>{
+ startPos.x(), startPos.y(), states.at(0).pose.angle() + initial_offset_angle};
+ state0.velocity = states.at(0).velocity + initial_offet_vel;
+
+ UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
+
+ std::vector<SystemModelT::StateT> ukf_states;
+ std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
+ ukf_states.push_back(state0);
+ ukf_Ps.push_back(P0);
+
+ std::vector<T> x_true, y_true, x_obs, y_obs, x_ukf, y_ukf;
+ std::vector<T> a_true, a_obs, a_ukf;
+ std::vector<T> vel_x_true, vel_y_true, vel_a_true;
+ std::vector<T> vel_x_ukf_full, vel_y_ukf_full, vel_a_ukf_full;
+ std::vector<T> x_ukf_full, y_ukf_full, a_ukf_full;
+ std::vector<T> ukf_pos_var, ukf_rot_var, ukf_var;
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ // propagate
+ TIMING_START(LOOP);
+ TIMING_START(PROPAGATION);
+ ukf.propagation(SystemModelT::ControlT{}, dt);
+ TIMING_END(PROPAGATION);
+ if ((i - 1) % 5 == 0)
+ {
+ TIMING_START(UPDATE);
+ ukf.update(observations.at(i));
+ TIMING_END(UPDATE);
+ TIMING_START(REST);
+
+ BOOST_TEST_MESSAGE("Max Cov "
+ << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+
+ x_obs.push_back(observations.at(i)(0));
+ y_obs.push_back(observations.at(i)(1));
+ a_obs.push_back(observations.at(i)(2));
+
+ x_ukf.push_back(ukf.getCurrentState().pose.x());
+ y_ukf.push_back(ukf.getCurrentState().pose.y());
+ a_ukf.push_back(ukf.getCurrentState().pose.angle());
+
+ TIMING_END(REST);
+ }
+
+ x_true.push_back(states.at(i).pose.x());
+ y_true.push_back(states.at(i).pose.y());
+ a_true.push_back(states.at(i).pose.angle());
+
+ vel_x_true.push_back(states.at(i).velocity.x());
+ vel_y_true.push_back(states.at(i).velocity.y());
+ vel_a_true.push_back(states.at(i).velocity.angle());
+
+ x_ukf_full.push_back(ukf.getCurrentState().pose.x());
+ y_ukf_full.push_back(ukf.getCurrentState().pose.y());
+ a_ukf_full.push_back(ukf.getCurrentState().pose.angle());
+
+ vel_x_ukf_full.push_back(ukf.getCurrentState().velocity.x());
+ vel_y_ukf_full.push_back(ukf.getCurrentState().velocity.y());
+ vel_a_ukf_full.push_back(ukf.getCurrentState().velocity.angle());
+
+ ukf_states.push_back(ukf.getCurrentState());
+ ukf_Ps.push_back(ukf.getCurrentStateCovariance());
+
+ ukf_pos_var.push_back(
+ ukf.getCurrentStateCovariance().block<2, 2>(0, 0).cwiseAbs().maxCoeff());
+ ukf_rot_var.push_back(
+ ukf.getCurrentStateCovariance().block<1, 1>(2, 2).cwiseAbs().maxCoeff());
+ ukf_var.push_back(ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+
+ TIMING_END(LOOP);
+ }
+
+ sciplot::Plot2D pos_plot;
+ pos_plot.xlabel("x");
+ pos_plot.ylabel("y");
+ pos_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ pos_plot.xrange(-RAD * 1.1, RAD * 1.1).yrange(-RAD * 1.1, RAD * 1.1);
+
+ pos_plot.drawCurve(x_true, y_true).label("True");
+ pos_plot.drawCurve(x_obs, y_obs).label("Obs");
+ pos_plot.drawCurveWithPoints(x_ukf, y_ukf).label("UKF");
+ pos_plot.drawCurve(x_ukf_full, y_ukf_full).label("Full UKF");
+
+
+ sciplot::Plot2D orientation_plot;
+ orientation_plot.xlabel("t");
+ orientation_plot.ylabel("rot");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange(-0.1, 1.1).yrange(-4, 4);
+
+ sciplot::Vec x = sciplot::linspace(0.0, 1.0, a_obs.size());
+ sciplot::Vec x_t = sciplot::linspace(0.0, 1.0, a_true.size());
+ orientation_plot.drawCurve(x_t, a_true).label("True").lineWidth(1);
+ orientation_plot.drawCurve(x, a_obs).label("Obs").lineWidth(1);
+ orientation_plot.drawCurve(x, a_ukf).label("UKF").lineWidth(1);
+ orientation_plot.drawCurve(x_t, a_ukf_full).label("Full UKF").lineWidth(1);
+
+
+ sciplot::Plot2D velocity_plot;
+ velocity_plot.xlabel("t");
+ velocity_plot.ylabel("vel");
+ velocity_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ velocity_plot.xrange(-0.1, 1.1).yrange("*", "*");
+
+ velocity_plot.drawCurve(x_t, vel_x_true).label("x True").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_y_true).label("y True").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_a_true).label("a True").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_x_ukf_full).label("x Ukf").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_y_ukf_full).label("y Ukf").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_a_ukf_full).label("a Ukf").lineWidth(1);
+
+
+ sciplot::Plot2D error_plot;
+ error_plot.xlabel("t");
+ error_plot.ylabel("err");
+ error_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ error_plot.xrange(-0.1, 1.1).yrange("*", "*");
+
+ error_plot.drawCurve(x_t, ukf_pos_var).label("Pos variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_rot_var).label("Rot variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_var).label("Total variance").lineWidth(1);
+
+
+ // Create figure to hold plot
+ sciplot::Figure fig = {{pos_plot, orientation_plot}, {velocity_plot, error_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(1000, 1000);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("se2xV_kalman_output.pdf");
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/so2kalmanFilterTest.cpp b/source/armarx/navigation/human/test/so2kalmanFilterTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0ae9fc51286acdf96379f13d1f85b4f24695fa9c
--- /dev/null
+++ b/source/armarx/navigation/human/test/so2kalmanFilterTest.cpp
@@ -0,0 +1,293 @@
+/**
+ * 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 )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/time.h>
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <manif/SO2.h>
+#include <sciplot/sciplot.hpp>
+
+namespace armarx::navigation::human
+{
+ using T = double; //TODO double or float?
+ using Vector = Eigen::Matrix<T, 2, 1>;
+ using SystemModelT = kalman_filter::SystemModelSO2xR2<T>;
+
+ constexpr T RAD = 40;
+ constexpr long num_timesteps = 3000;
+ constexpr T max_time = 1;
+ constexpr T dt = max_time / num_timesteps;
+ constexpr T c = (1 / max_time) * 2 * M_PI; // rotation per time
+
+ constexpr T rot_noise_std = 0.2;
+ constexpr T pos_noise_std = 5;
+ constexpr T obs_rot_noise_std = 0.2;
+ constexpr T obs_pos_noise_std = 5;
+ constexpr T initial_offset_angle = 1 * 10 * M_PI / 180;
+ const Vector initial_offet_pos = 0.8 * RAD * Vector(1, 0.5);
+
+
+ void
+ simulate_trajectory(std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ControlT>& omegas)
+ {
+ constexpr T startAngle = 0;
+
+ SystemModelT::StateT state;
+ state.position = Vector{RAD, 0};
+ state.orientation = manif::SO2<T>(startAngle);
+ states.push_back(state);
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T angle = t * c;
+
+ const Vector last_pos = states.at(i - 1).position;
+ //const Vector pos(angle, std::cos(angle));
+ const Vector pos = RAD * Vector(std::cos(angle), std::sin(angle));
+
+ BOOST_TEST(((last_pos - pos).norm() < 0.1 * RAD),
+ "position differs too much from last step: " << (last_pos - pos).norm());
+
+ SystemModelT::ControlT control;
+ //TODO (SALt): check angular velocity (>pi overflow)
+ control.angular_velocity.coeffs() << c; //(last_angle - angle) / dt;
+ control.euclidean_velocity << (pos - last_pos) / dt;
+ //control.euclidean_velocity = Eigen::Vector2d{0, c};
+
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
+ state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+
+ state.position = pos;
+ state.orientation = manif::SO2<T>(angle + startAngle);
+
+ T pos_diff = (propagated.position - state.position).norm();
+ BOOST_TEST(pos_diff < 2e-10,
+ "propagated position differs too much from real position: " << pos_diff);
+
+ T rot_diff = propagated.orientation.angle() - state.orientation.angle();
+ BOOST_TEST(rot_diff < 2e-10,
+ "propagated rotation differs too much from real orientation: " << rot_diff);
+
+ states.push_back(propagated);
+
+
+ // add noise
+ control.angular_velocity.coeffs() += rot_noise_std * Eigen::Matrix<T, 1, 1>::Random();
+ control.euclidean_velocity += pos_noise_std * Vector::Random();
+ omegas.push_back(control);
+ }
+ }
+
+ void
+ simulate_observation(const std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ObsT>& observations)
+ {
+ for (const auto& state : states)
+ {
+ SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
+ SystemModelT::ObsT true_obs;
+ true_obs.segment(0, 1) = state.orientation.log().coeffs();
+ true_obs.segment(1, 2) = state.position;
+
+ BOOST_TEST(
+ (obs - true_obs).norm() < std::numeric_limits<T>::epsilon(),
+ "observation differs too much from real observation: " << (obs - true_obs).norm());
+
+ SystemModelT::ObsT noise = SystemModelT::ObsT::Random();
+ noise.segment(0, 1) *= obs_rot_noise_std;
+ noise.segment(1, 2) *= obs_pos_noise_std;
+
+ observations.push_back(obs + noise);
+ }
+ }
+
+
+ BOOST_AUTO_TEST_CASE(so2KalmanFilterTest)
+ {
+ srand(time(NULL));
+
+ std::vector<SystemModelT::StateT> states;
+ std::vector<SystemModelT::ControlT> omegas;
+ std::vector<SystemModelT::ObsT> observations;
+ simulate_trajectory(states, omegas);
+ simulate_observation(states, observations);
+
+ ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size()
+ << " Num Obs: " << observations.size();
+
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<1, 1>(0, 0) *= rot_noise_std * rot_noise_std;
+ Q.block<2, 2>(1, 1) *= pos_noise_std * pos_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<1, 1>(0, 0) *= obs_rot_noise_std * obs_rot_noise_std;
+ R.block<2, 2>(1, 1) *= obs_pos_noise_std * obs_pos_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<1, 1>(0, 0) *= initial_offset_angle * initial_offset_angle;
+ P0.block<2, 2>(1, 1) *= initial_offet_pos.norm() * initial_offet_pos.norm();
+
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha;
+ alpha(0) = 0.5; // state
+ alpha(1) = 0.5; // control
+ alpha(2) = 0.5; // update
+
+ SystemModelT::StateT state0;
+ state0.orientation = manif::SO2<T>(states.at(0).orientation.angle() + initial_offset_angle);
+ state0.position = states.at(0).position + initial_offet_pos;
+
+ UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
+
+ std::vector<SystemModelT::StateT> ukf_states;
+ std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
+ ukf_states.push_back(state0);
+ ukf_Ps.push_back(P0);
+
+ std::vector<T> x_true, y_true, x_obs, y_obs, x_ukf, y_ukf;
+ std::vector<T> a_true, a_obs, a_ukf;
+ std::vector<T> x_ukf_full, y_ukf_full, a_ukf_full;
+ std::vector<T> ukf_pos_var, ukf_rot_var, ukf_var;
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ // propagate
+ TIMING_START(LOOP);
+ TIMING_START(PROPAGATION);
+ ukf.propagation(omegas.at(i - 1), dt);
+ TIMING_END(PROPAGATION);
+ if ((i - 1) % 50 == 0)
+ {
+ TIMING_START(UPDATE);
+ ukf.update(observations.at(i));
+ TIMING_END(UPDATE);
+ TIMING_START(REST);
+ const SystemModelT::StateT& current_state = ukf.getCurrentState();
+
+ BOOST_TEST_MESSAGE(
+ "Position Diff: " << (states.at(i).position - current_state.position).norm());
+ BOOST_TEST_MESSAGE(
+ "Orientation Diff: "
+ << (states.at(i).orientation - current_state.orientation).coeffs().norm());
+
+ BOOST_TEST_MESSAGE("Max Cov "
+ << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+ BOOST_TEST_MESSAGE("Diag: " << (ukf.getCurrentStateCovariance() -
+ Eigen::Matrix<T, 3, 3>::Identity())
+ .norm());
+
+ x_obs.push_back(observations.at(i)(1));
+ y_obs.push_back(observations.at(i)(2));
+ a_obs.push_back(observations.at(i)(0));
+
+ x_ukf.push_back(ukf.getCurrentState().position.x());
+ y_ukf.push_back(ukf.getCurrentState().position.y());
+ a_ukf.push_back(ukf.getCurrentState().orientation.log().coeffs()(0));
+
+ TIMING_END(REST);
+ }
+ x_true.push_back(states.at(i).position.x());
+ y_true.push_back(states.at(i).position.y());
+ a_true.push_back(states.at(i).orientation.log().coeffs()(0));
+
+ x_ukf_full.push_back(ukf.getCurrentState().position.x());
+ y_ukf_full.push_back(ukf.getCurrentState().position.y());
+ a_ukf_full.push_back(ukf.getCurrentState().orientation.log().coeffs()(0));
+
+ ukf_states.push_back(ukf.getCurrentState());
+ ukf_Ps.push_back(ukf.getCurrentStateCovariance());
+
+ ukf_pos_var.push_back(
+ ukf.getCurrentStateCovariance().block<2, 2>(1, 1).cwiseAbs().maxCoeff());
+ ukf_rot_var.push_back(
+ ukf.getCurrentStateCovariance().block<1, 1>(0, 0).cwiseAbs().maxCoeff());
+ ukf_var.push_back(ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+
+ TIMING_END(LOOP);
+ }
+
+ sciplot::Plot2D pos_plot;
+ pos_plot.xlabel("x");
+ pos_plot.ylabel("y");
+ pos_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ pos_plot.xrange(-RAD * 1.1, RAD * 1.1).yrange(-RAD * 1.1, RAD * 1.1);
+ //pos_plot.xrange(-4, 4).yrange(-4, 4);
+
+ pos_plot.drawCurve(x_true, y_true).label("True");
+ pos_plot.drawCurve(x_obs, y_obs).label("Obs");
+ pos_plot.drawCurveWithPoints(x_ukf, y_ukf).label("UKF");
+ pos_plot.drawCurve(x_ukf_full, y_ukf_full).label("Full UKF");
+
+
+ sciplot::Plot2D orientation_plot;
+ orientation_plot.xlabel("x");
+ orientation_plot.ylabel("y");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange(-0.1, 1.1).yrange(-4, 4);
+
+ sciplot::Vec x = sciplot::linspace(0.0, 1.0, a_obs.size());
+ sciplot::Vec x_t = sciplot::linspace(0.0, 1.0, a_true.size());
+ orientation_plot.drawCurve(x_t, a_true).label("True").lineWidth(1);
+ orientation_plot.drawCurve(x, a_obs).label("Obs").lineWidth(1);
+ orientation_plot.drawCurve(x, a_ukf).label("UKF").lineWidth(1);
+ orientation_plot.drawCurve(x_t, a_ukf_full).label("Full UKF").lineWidth(1);
+
+
+ sciplot::Plot2D error_plot;
+ error_plot.xlabel("x");
+ error_plot.ylabel("y");
+ error_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ error_plot.xrange(-0.1, 1.1).yrange("*", "*");
+
+ error_plot.drawCurve(x_t, ukf_pos_var).label("Pos variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_rot_var).label("Rot variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_var).label("Total variance").lineWidth(1);
+
+
+ // Create figure to hold plot
+ sciplot::Figure fig = {{pos_plot, orientation_plot}, {error_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(1000, 1000);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("so2_kalman_output.pdf");
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/types.cpp b/source/armarx/navigation/human/types.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..854b0d15569474955bde4550b77c6a168aa8238e
--- /dev/null
+++ b/source/armarx/navigation/human/types.cpp
@@ -0,0 +1,58 @@
+#include "types.h"
+
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/aron_conversions.h>
+
+namespace armarx::navigation::human
+{
+ core::Pose2D
+ Human::estimateAt(const DateTime& time) const
+ {
+ double dt = (time - detectionTime).toSecondsDouble();
+ core::Pose2D estimation{pose};
+ estimation.translation() += linearVelocity * dt;
+ return estimation;
+ }
+
+ aron::data::DictPtr
+ Human::toAron() const
+ {
+ arondto::Human dto;
+ human::toAron(dto, *this);
+ return dto.toAron();
+ }
+
+ Human
+ Human::FromAron(const aron::data::DictPtr& dict)
+ {
+ ARMARX_CHECK_NOT_NULL(dict);
+ arondto::Human dto;
+ dto.fromAron(dict);
+
+ Human bo;
+ fromAron(dto, bo);
+ return bo;
+ }
+
+
+ aron::data::DictPtr
+ HumanGroup::toAron() const
+ {
+ arondto::HumanGroup dto;
+ human::toAron(dto, *this);
+ return dto.toAron();
+ }
+
+ HumanGroup
+ HumanGroup::FromAron(const aron::data::DictPtr& dict)
+ {
+ ARMARX_CHECK_NOT_NULL(dict);
+ arondto::HumanGroup dto;
+ dto.fromAron(dict);
+
+ HumanGroup bo;
+ fromAron(dto, bo);
+ return bo;
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..0c3e08043b9fcea45aa603a0ef01d0eeba87f63d
--- /dev/null
+++ b/source/armarx/navigation/human/types.h
@@ -0,0 +1,84 @@
+/**
+ * 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 )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/time.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/shapes.h>
+
+namespace armarx::navigation::human
+{
+ struct Human
+ {
+ core::Pose2D pose;
+ Eigen::Vector2f linearVelocity;
+ DateTime detectionTime;
+
+ core::Pose2D estimateAt(const DateTime& time) const;
+
+ aron::data::DictPtr toAron() const;
+ static Human FromAron(const aron::data::DictPtr& dict);
+ };
+
+ using Humans = std::vector<Human>;
+
+
+ struct HumanGroup
+ {
+ shapes::Polygon shape;
+ Humans humans;
+ DateTime detectionTime;
+
+ aron::data::DictPtr toAron() const;
+ static HumanGroup FromAron(const aron::data::DictPtr& dict);
+ };
+
+ using HumanGroups = std::vector<HumanGroup>;
+
+ struct LinearPenaltyModel
+ {
+ float minDistance; // [m]
+ float epsilon; // [m]
+ };
+
+ struct ExponentialPenaltyModel
+ {
+ float minDistance; // [m]
+ float epsilon; // [m]
+ float exponent;
+ };
+
+ struct ProxemicZone
+ {
+ core::Pose2D pose;
+ shapes::Ellipse shape;
+ ExponentialPenaltyModel penalty;
+ float weight;
+ bool homotopicRelevance = true;
+ };
+
+ using ProxemicZones = std::vector<ProxemicZone>;
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/local_planning/CMakeLists.txt b/source/armarx/navigation/local_planning/CMakeLists.txt
index d857c412ffb26d86e80b5cc0fd2c5fdf40d91754..fa3f62b422d975707ea1d1c7424e3e4cdc898de2 100644
--- a/source/armarx/navigation/local_planning/CMakeLists.txt
+++ b/source/armarx/navigation/local_planning/CMakeLists.txt
@@ -1,14 +1,67 @@
+armarx_add_aron_library(local_planning_aron
+ ARON_FILES
+ aron/TimedElasticBands.xml
+)
-armarx_add_library(local_planning
- DEPENDENCIES
+
+armarx_add_library(local_planning_core
+ DEPENDENCIES_PUBLIC
ArmarXCoreInterfaces
ArmarXCore
+ ArViz
armarx_navigation::core
- SOURCES
- ./LocalPlanner.cpp
- ./TimedElasticBands.cpp
+ armarx_navigation::conversions
+ armarx_navigation::algorithms
+ armarx_navigation::local_planning_aron
+ DEPENDENCIES_PRIVATE
+ range-v3::range-v3
+ SOURCES
+ LocalPlanner.cpp
+ aron_conversions.cpp
+ TimedElasticBandsParams.cpp
HEADERS
- ./LocalPlanner.h
- ./TimedElasticBands.h
+ LocalPlanner.h
+ TimedElasticBandsParams.h
core.h
+ aron_conversions.h
+ OBJECT
+)
+
+armarx_add_library(local_planning_teb
+ DEPENDENCIES_PUBLIC
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArViz
+ armarx_navigation::local_planning_core
+ armarx_navigation::teb_human
+ teb_extension::obstacles
+ DEPENDENCIES_PRIVATE
+ range-v3::range-v3
+ DEPENDENCIES_LEGACY
+ teb_local_planner
+ SOURCES
+ TimedElasticBands.cpp
+ TebObstacleManager.cpp
+ ros_conversions.cpp
+ aron_conversions_teb.cpp
+ HEADERS
+ TimedElasticBands.h
+ TebObstacleManager.h
+ ros_conversions.h
+ aron_conversions_teb.h
+ OBJECT
+)
+
+target_compile_definitions(local_planning_teb PUBLIC TIMED_ELASTIC_BANDS_ENABLED=1)
+
+if(${teb_local_planner_FOUND})
+ set(local_planning_optional_deps armarx_navigation::local_planning_teb)
+endif()
+
+armarx_add_library(local_planning
+ DEPENDENCIES_PUBLIC
+ armarx_navigation::local_planning_core
+ ${local_planning_optional_deps}
+ DEPENDENCIES_PRIVATE
+ range-v3::range-v3
)
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.cpp b/source/armarx/navigation/local_planning/LocalPlanner.cpp
index 7205dd808f91089a5a750d5973599bc74bbdaced..ab0fbd229acf07ea26833e9a77481fe923f6237e 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.cpp
+++ b/source/armarx/navigation/local_planning/LocalPlanner.cpp
@@ -1,8 +1,14 @@
#include "LocalPlanner.h"
-namespace armarx::navigation::loc_plan
+namespace armarx::navigation::local_planning
{
LocalPlanner::LocalPlanner(const core::Scene& context) : context(context)
{
}
-} // namespace armarx::navigation::loc_plan
+
+ void
+ LocalPlanner::setVisualization(viz::Client& 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 5e7d06bf7ea787aa88e97d517e907b099554a012..031318818e623d942d7acd3469d5bf771722890f 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.h
+++ b/source/armarx/navigation/local_planning/LocalPlanner.h
@@ -26,19 +26,20 @@
#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include "core.h"
#include <armarx/navigation/core/DynamicScene.h>
#include <armarx/navigation/core/StaticScene.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/local_planning/core.h>
-namespace armarx::navigation::loc_plan
+namespace armarx::navigation::local_planning
{
struct LocalPlannerResult
{
- core::Trajectory trajectory;
+ core::LocalTrajectory trajectory;
};
struct LocalPlannerParams
@@ -55,13 +56,17 @@ namespace armarx::navigation::loc_plan
LocalPlanner(const core::Scene& context);
virtual ~LocalPlanner() = default;
- virtual std::optional<LocalPlannerResult> plan(const core::Trajectory& goal) = 0;
+ virtual std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) = 0;
+
+ void setVisualization(viz::Client& vis);
protected:
+ std::optional<viz::ScopedClient> arviz;
+
private:
const core::Scene& context;
};
using LocalPlannerPtr = std::shared_ptr<LocalPlanner>;
-} // namespace armarx::navigation::loc_plan
+} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e16fffd374d72663e51c6c8609856b88565b68ad
--- /dev/null
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -0,0 +1,102 @@
+#include "TebObstacleManager.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/local_planning/ros_conversions.h>
+#include <teb_local_planner/extension/obstacles/EllipseObstacle.h>
+#include <teb_local_planner/obstacles.h>
+
+namespace armarx::navigation::local_planning
+{
+
+ void
+ TebObstacleManager::clear()
+ {
+ container.clear();
+ visualizationIndex = 0;
+ }
+
+ size_t
+ TebObstacleManager::size()
+ {
+ return container.size();
+ }
+
+ void
+ TebObstacleManager::addBoxObstacle(const VirtualRobot::BoundingBox& bbox, viz::Layer* visLayer)
+ {
+ auto obst = boost::make_shared<teb_local_planner::PolygonObstacle>();
+
+ const Eigen::Vector2d min = conv::toRos(bbox.getMin());
+ const Eigen::Vector2d max = conv::toRos(bbox.getMax());
+
+ obst->pushBackVertex(min);
+ obst->pushBackVertex(min.x(), max.y());
+ obst->pushBackVertex(max);
+ obst->pushBackVertex(max.x(), min.y());
+
+ obst->finalizePolygon();
+ obst->setUseForOptimization(true);
+ container.push_back(obst);
+
+ // visualize bounding box if layer is available
+ if (visLayer != nullptr)
+ {
+ const Eigen::Vector3f min3d = conv::fromRos(min);
+ const Eigen::Vector3f max3d = conv::fromRos(max);
+
+ visLayer->add(viz::Polygon("polygon_" + std::to_string(visualizationIndex++))
+ .addPoint(min3d)
+ .addPoint(Eigen::Vector3f(min3d.x(), max3d.y(), 0))
+ .addPoint(max3d)
+ .addPoint(Eigen::Vector3f(max3d.x(), min3d.y(), 0))
+ .color(simox::Color::gray()));
+ }
+ }
+
+ void
+ TebObstacleManager::addHumanObstacle(const human::Human& human, viz::Layer* visLayer)
+ {
+ auto proxemicZones = proxemics.createProxemicZones(human);
+
+ int i = 0;
+ for (const auto& proxemicZone : proxemicZones)
+ {
+ auto pose = conv::toRos(proxemicZone.pose);
+ auto shape = conv::toRos(proxemicZone.shape);
+
+ auto obst = boost::make_shared<teb_local_planner::extension::EllipseObstacle>(
+ pose.position(), pose.theta(), shape.a, shape.b);
+
+ const auto& penalty = proxemicZone.penalty;
+
+ obst->setPenaltyModel(conv::toRos(penalty));
+
+ obst->setWeight(proxemicZone.weight);
+ obst->setHomotopicRelevance(proxemicZone.homotopicRelevance);
+
+ if (human.linearVelocity != Eigen::Vector2f::Zero())
+ {
+ obst->setCentroidVelocity(conv::toRos2D(human.linearVelocity));
+ }
+
+ container.push_back(obst);
+
+ // visualize proxemic zone if layer is available
+ if (visLayer != nullptr)
+ {
+ 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(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]));
+ }
+ i++;
+ }
+ }
+
+} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.h b/source/armarx/navigation/local_planning/TebObstacleManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..b8859c4bbcb8145c3e1bb159e3ba7490331b14e4
--- /dev/null
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.h
@@ -0,0 +1,59 @@
+/**
+ * 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 )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+
+#include <armarx/navigation/human/ProxemicZoneCreator.h>
+#include <armarx/navigation/human/types.h>
+#include <teb_local_planner/obstacles.h>
+
+namespace armarx::navigation::local_planning
+{
+
+
+ class TebObstacleManager
+ {
+ public:
+ TebObstacleManager(teb_local_planner::ObstContainer& container) : container(container)
+ {
+ }
+
+ void clear();
+
+ size_t size();
+
+ void addBoxObstacle(const VirtualRobot::BoundingBox& bbox, viz::Layer* visLayer = nullptr);
+ void addHumanObstacle(const human::Human& human, viz::Layer* visLayer = nullptr);
+
+ private:
+ 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()};
+ };
+
+
+} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index b069cba6334265022ee887d9ce41f0cd0be401d1..1430129d43e00729240f602cd8c7e597a369486e 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -1,42 +1,220 @@
#include "TimedElasticBands.h"
+#include <optional>
+
+#include <SimoxUtility/algorithm/apply.hpp>
+#include <SimoxUtility/json/json.hpp>
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/SceneObjectSet.h>
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h"
+
+#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
+#include <armarx/navigation/local_planning/aron/TimedElasticBands.aron.generated.h>
+#include <armarx/navigation/local_planning/aron_conversions.h>
+#include <armarx/navigation/local_planning/aron_conversions_teb.h>
#include <armarx/navigation/local_planning/core.h>
+#include <armarx/navigation/local_planning/ros_conversions.h>
+// #include <teb_local_planner/extension/obstacles/EllipseObstacle.h>
+#include <armarx/navigation/local_planning/aron_conversions_teb.h>
+#include <teb_local_planner/homotopy_class_planner.h>
-namespace armarx::navigation::loc_plan
+namespace armarx::navigation::local_planning
{
- // TimedElasticBandsParams
+ // TimedElasticBands
- Algorithms
- TimedElasticBandsParams::algorithm() const
+ TimedElasticBands::TimedElasticBands(const Params& i_params, const core::Scene& ctx) :
+ LocalPlanner(ctx), params(i_params), scene(ctx), obstManager(teb_obstacles)
{
- return Algorithms::TimedElasticBands;
+ //TODO (SALt): find proper place to init with default config
+ if (true)
+ {
+ readDefaultConfig(params.cfg);
+ }
+ toTebCfg(params.cfg, cfg_);
+
+ auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(
+ params.cfg.robot_footprint_radius);
+
+ ARMARX_VERBOSE << "Robot footprint is " << params.cfg.robot_footprint_radius << "m";
+
+ hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>();
+ hcp_->initialize(
+ cfg_, &teb_obstacles, robot_model, teb_local_planner::TebVisualizationPtr(), nullptr);
+ //set member teb_costmap
+ setTebCostmap();
+ if (teb_costmap)
+ {
+ ARMARX_VERBOSE << "Costmap available.";
+ // TODO: where to put all the parameters
+ const human::ExponentialPenaltyModel penalty{
+ .minDistance = 0.5, .epsilon = 0, .exponent = 1.2};
+
+ teb_costmap->setPenaltyModel(conv::toRos(penalty));
+ hcp_->setCostmap(&teb_costmap.value());
+ }
+ ros::Time::init(); // we have to init time before we can use the planner
}
- aron::data::DictPtr
- TimedElasticBandsParams::toAron() const
+ void
+ TimedElasticBands::readDefaultConfig(arondto::TimedElasticBandsParams& target)
{
- return nullptr; // TODO implement
+ const armarx::PackagePath configPath("armarx_navigation",
+ "local_planner_config/TimedElasticBands/default.json");
+ const std::filesystem::path file = configPath.toSystemPath();
+
+ ARMARX_CHECK(std::filesystem::is_regular_file(file)) << file;
+
+ ARMARX_INFO << "Loading config from file `" << file << "`.";
+ std::ifstream ifs{file};
+
+ nlohmann::json jsonConfig;
+ ifs >> jsonConfig;
+
+ ARMARX_INFO << "Reading config";
+
+ armarx::aron::data::reader::NlohmannJSONReaderWithoutTypeCheck reader;
+ target.read(reader, jsonConfig);
}
- TimedElasticBandsParams
- TimedElasticBandsParams::FromAron(const aron::data::DictPtr& dict)
+ std::optional<LocalPlannerResult>
+ TimedElasticBands::plan(const core::GlobalTrajectory& goal)
{
- return TimedElasticBandsParams(); // TODO implement
- }
+ TIMING_START(TEB_PLAN);
+ const core::Pose currentPose{scene.robot->getGlobalPose()};
- // TimedElasticBands
+ // prune global trajectory
+ const auto& [prunedGoal, planToDest] = goal.getSubTrajectory(
+ currentPose.translation(), params.cfg.planning_distance * 1000); // [m] to [mm]
+
+ const teb_local_planner::TimedElasticBand globalPath = conv::toRos(prunedGoal);
+ teb_globalPath = globalPath.poses();
+ hcp_->setGlobalPath(&teb_globalPath);
+
+ const teb_local_planner::PoseSE2 start = conv::toRos(currentPose);
+ const teb_local_planner::PoseSE2 end =
+ conv::toRos(prunedGoal.points().back().waypoint.pose);
+
+ geometry_msgs::Twist velocity_start = conv::toRos(scene.platformVelocity);
- TimedElasticBands::TimedElasticBands(const Params& params, const core::Scene& ctx) :
- LocalPlanner(ctx), params(params)
+
+ fillObstacles();
+
+ try
+ {
+ hcp_->plan(start, end, &velocity_start, !planToDest);
+ }
+ catch (std::exception& e)
+ {
+ ARMARX_ERROR << "Caugth exception while planning: " << e.what();
+ return std::nullopt;
+ }
+
+ if (hcp_->getTrajectoryContainer().empty())
+ {
+ ARMARX_INFO << deactivateSpam(5) << "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)
+ {
+ auto layer = arviz.value().layer("local_planner_path_alternatives");
+
+ int i = 0;
+ int bestTebIdx = hcp_->bestTebIdx();
+ for (const auto& teb : hcp_->getTrajectoryContainer())
+ {
+ if (i == bestTebIdx)
+ {
+ continue;
+ }
+ const core::LocalTrajectory trajectory = conv::fromRos(teb->teb());
+ const std::vector<Eigen::Vector3f> points =
+ simox::alg::apply(trajectory.points(),
+ [](const core::LocalTrajectoryPoint& pt) -> Eigen::Vector3f
+ { return pt.pose.translation(); });
+ layer.add(viz::Path("path_alternative_" + std::to_string(i))
+ .points(points)
+ .color(simox::Color::gray()));
+ i++;
+ }
+
+ arviz.value().commit(layer);
+ }
+
+ TIMING_END_COMMENT_STREAM(TEB_PLAN, "Timer: teb planning", ARMARX_VERBOSE);
+ return {{.trajectory = best}};
+ }
+
+ void
+ TimedElasticBands::fillObstacles()
{
+ obstManager.clear();
+
+ viz::Layer* visPtr = nullptr;
+ viz::Layer visLayer;
+ if (arviz)
+ {
+ visLayer = arviz.value().layer("local_planner_obstacles");
+ visPtr = &visLayer;
+ }
+
+
+ if (scene.staticScene)
+ {
+ ARMARX_CHECK(scene.staticScene.has_value());
+ for (const auto& obst : scene.staticScene.value().objects->getCollisionModels())
+ {
+ obstManager.addBoxObstacle(obst->getGlobalBoundingBox(), visPtr);
+ }
+ }
+ if (scene.dynamicScene)
+ {
+ ARMARX_CHECK(scene.dynamicScene.has_value());
+ for (const auto& obst : scene.dynamicScene.value().humans)
+ {
+ obstManager.addHumanObstacle(obst, visPtr);
+ }
+ }
+
+ if (arviz)
+ {
+ arviz.value().commit(visLayer);
+ }
+
+ ARMARX_VERBOSE << "TEB: added " << obstManager.size() << " obstacles";
}
- std::optional<LocalPlannerResult>
- TimedElasticBands::plan(const core::Trajectory& goal)
+ //export algorithms::Costmap to costmap type of teb local planner and provide costmap to planner
+ void
+ TimedElasticBands::setTebCostmap()
{
- // TODO implement
- return {};
+ if (not scene.staticScene.has_value())
+ {
+ return;
+ }
+
+ if (not scene.staticScene->distanceToObstaclesCostmap.has_value())
+ {
+ return;
+ }
+
+ const algorithms::Costmap& navigationCostmap =
+ scene.staticScene->distanceToObstaclesCostmap.value();
+ teb_costmap.emplace(conv::toRos(navigationCostmap));
}
-} // namespace armarx::navigation::loc_plan
+
+} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index 8b965d154ad4a997ff530f5adf73083386461dc1..420452395e2ba475e9257cd3ccff22feb3864675 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -24,24 +24,17 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
+#include <armarx/navigation/local_planning/TebObstacleManager.h>
+#include <armarx/navigation/local_planning/TimedElasticBandsParams.h>
#include <armarx/navigation/local_planning/core.h>
+#include <teb_local_planner/homotopy_class_planner.h>
+#include <teb_local_planner/teb_config.h>
-namespace armarx::navigation::loc_plan
+namespace armarx::navigation::local_planning
{
- // TODO(SALt): Implement
-
- struct TimedElasticBandsParams : public LocalPlannerParams
- {
- bool includeStartPose{false};
-
-
- Algorithms algorithm() const override;
- aron::data::DictPtr toAron() const override;
- static TimedElasticBandsParams FromAron(const aron::data::DictPtr& dict);
- };
-
class TimedElasticBands : virtual public LocalPlanner
{
public:
@@ -50,13 +43,26 @@ namespace armarx::navigation::loc_plan
TimedElasticBands(const Params& params, const core::Scene& ctx);
~TimedElasticBands() override = default;
- void init(const core::Trajectory& initialTrajectory);
+ void init(const core::GlobalTrajectory& initialTrajectory);
+
+ std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) override;
- std::optional<LocalPlannerResult> plan(const core::Trajectory& goal) override;
+ private:
+ void readDefaultConfig(arondto::TimedElasticBandsParams& target);
+ void fillObstacles();
+ void setTebCostmap();
protected:
- const Params params;
+ Params params;
private:
+ const core::Scene& scene;
+
+ teb_local_planner::TebConfig cfg_;
+ teb_local_planner::ObstContainer teb_obstacles;
+ TebObstacleManager obstManager;
+ teb_local_planner::PoseSequence teb_globalPath;
+ std::optional<teb_local_planner::Costmap> teb_costmap;
+ std::unique_ptr<teb_local_planner::HomotopyClassPlanner> hcp_{nullptr};
};
-} // namespace armarx::navigation::loc_plan
+} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/TimedElasticBandsParams.cpp b/source/armarx/navigation/local_planning/TimedElasticBandsParams.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c722983579ebb3061024a1a5a604910d4a4abf7a
--- /dev/null
+++ b/source/armarx/navigation/local_planning/TimedElasticBandsParams.cpp
@@ -0,0 +1,29 @@
+#include "TimedElasticBandsParams.h"
+
+namespace armarx::navigation::local_planning
+{
+ // TimedElasticBandsParams
+
+ Algorithms
+ TimedElasticBandsParams::algorithm() const
+ {
+ return Algorithms::TimedElasticBands;
+ }
+
+ aron::data::DictPtr
+ TimedElasticBandsParams::toAron() const
+ {
+ return cfg.toAron();
+ }
+
+ TimedElasticBandsParams
+ TimedElasticBandsParams::FromAron(const aron::data::DictPtr& dict)
+ {
+ ARMARX_CHECK_NOT_NULL(dict);
+
+ TimedElasticBandsParams bo;
+ bo.cfg.fromAron(dict);
+
+ return bo;
+ }
+}
diff --git a/source/armarx/navigation/local_planning/TimedElasticBandsParams.h b/source/armarx/navigation/local_planning/TimedElasticBandsParams.h
new file mode 100644
index 0000000000000000000000000000000000000000..5dcc0dc61ae640df5c2353f22491605fda032ed4
--- /dev/null
+++ b/source/armarx/navigation/local_planning/TimedElasticBandsParams.h
@@ -0,0 +1,44 @@
+/**
+ * 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 )
+ * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+
+#include <armarx/navigation/local_planning/LocalPlanner.h>
+#include <armarx/navigation/local_planning/aron/TimedElasticBands.aron.generated.h>
+#include <armarx/navigation/local_planning/core.h>
+
+namespace armarx::navigation::local_planning
+{
+
+ struct TimedElasticBandsParams : public LocalPlannerParams
+ {
+ arondto::TimedElasticBandsParams cfg;
+
+ Algorithms algorithm() const override;
+ aron::data::DictPtr toAron() const override;
+ static TimedElasticBandsParams FromAron(const aron::data::DictPtr& dict);
+ };
+
+} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9d0873c3669327ef28f03d3b05cdd4dbffa1f1d5
--- /dev/null
+++ b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
@@ -0,0 +1,412 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ <!-- <Include include="armarx/navigation/local_planning/aron/LocalPlannerParams.xml" /> -->
+ </AronIncludes>
+
+ <GenerateTypes>
+ <Object name='armarx::navigation::local_planning::arondto::pse'>
+ <ObjectChild key='pse_costum_obstacle_penalties'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='pse_costum_obstacle_penalties_dynamic'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='weight_costmap'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_global_path_position'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_global_path_orientation'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='lrk_use_alternative_approach'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='lrk_enable_collision_check'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='hybrid_homotopy_calculation'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='robot_diff_circumscribed_inscribed_radius'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::trajectory'>
+ <ObjectChild key='teb_autosize'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='dt_ref'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='dt_hysteresis'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='min_samples'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='max_samples'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='global_plan_overwrite_orientation'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='allow_init_with_backwards_motion'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='exact_arc_length'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='force_reinit_new_goal_dist'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='force_reinit_new_goal_angular'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='feasibility_check_no_poses'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='feasibility_check_lookahead_distance'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='min_resolution_collision_check_angular'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::robot'>
+ <ObjectChild key='max_vel_x'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='max_vel_x_backwards'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='max_vel_y'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='max_vel_trans'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='max_vel_theta'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='acc_lim_x'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='acc_lim_y'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='acc_lim_theta'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='min_turning_radius'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::goal_tolerance'>
+ <ObjectChild key='xy_goal_tolerance'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::obstacles'>
+ <ObjectChild key='min_obstacle_dist'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='inflation_dist'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='dynamic_obstacle_inflation_dist'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='include_dynamic_obstacles'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_association_force_inclusion_factor'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_association_cutoff_factor'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_proximity_ratio_max_vel'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_proximity_lower_bound'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_proximity_upper_bound'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::optim'>
+ <ObjectChild key='no_inner_iterations'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='no_outer_iterations'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='optimization_activate'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='optimization_verbose'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='penalty_epsilon'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_max_vel_x'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_max_vel_y'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_max_vel_theta'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_acc_lim_x'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_acc_lim_y'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_acc_lim_theta'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_kinematics_nh'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_kinematics_forward_drive'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_kinematics_turning_radius'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_optimaltime'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_shortest_path'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_obstacle'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_inflation'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_dynamic_obstacle'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_dynamic_obstacle_inflation'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_velocity_obstacle_ratio'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_viapoint'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_prefer_rotdir'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_adapt_factor'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_cost_exponent'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::hcp'>
+ <ObjectChild key='enable_multithreading'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='simple_exploration'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='max_number_classes'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='max_number_plans_in_current_class'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='selection_cost_hysteresis'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='selection_obst_cost_scale'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='selection_viapoint_cost_scale'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='selection_alternative_time_cost'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='selection_dropping_probability'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='switching_blocking_period'>
+ <float />
+ </ObjectChild>
+
+
+ <ObjectChild key='obstacle_heading_threshold'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='roadmap_graph_no_samples'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='roadmap_graph_area_width'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='roadmap_graph_area_length_scale'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='h_signature_prescaler'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='h_signature_threshold'>
+ <float />
+ </ObjectChild>
+
+
+ <ObjectChild key='viapoints_all_candidates'>
+ <bool />
+ </ObjectChild>
+
+
+ <ObjectChild key='delete_detours_backwards'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='detours_orientation_tolerance'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='length_start_orientation_vector'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='max_ratio_detours_duration_best_duration'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::teb_config'>
+ <ObjectChild key='pse'>
+ <armarx::navigation::local_planning::arondto::pse />
+ </ObjectChild>
+ <ObjectChild key='trajectory'>
+ <armarx::navigation::local_planning::arondto::trajectory />
+ </ObjectChild>
+ <ObjectChild key='robot'>
+ <armarx::navigation::local_planning::arondto::robot />
+ </ObjectChild>
+ <ObjectChild key='goal_tolerance'>
+ <armarx::navigation::local_planning::arondto::goal_tolerance />
+ </ObjectChild>
+ <ObjectChild key='obstacles'>
+ <armarx::navigation::local_planning::arondto::obstacles />
+ </ObjectChild>
+ <ObjectChild key='optim'>
+ <armarx::navigation::local_planning::arondto::optim />
+ </ObjectChild>
+ <ObjectChild key='hcp'>
+ <armarx::navigation::local_planning::arondto::hcp />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams'>
+ <ObjectChild key='robot_footprint_radius'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='planning_distance'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='teb_config'>
+ <armarx::navigation::local_planning::arondto::teb_config />
+ </ObjectChild>
+ </Object>
+
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/local_planning/aron_conversions.cpp b/source/armarx/navigation/local_planning/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..140c02cb611f49a245a3e6b46bdf11979cdc97b2
--- /dev/null
+++ b/source/armarx/navigation/local_planning/aron_conversions.cpp
@@ -0,0 +1,25 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+#include <armarx/navigation/local_planning/TimedElasticBandsParams.h>
+
+namespace armarx::navigation::local_planning
+{
+
+
+ void
+ toAron(arondto::TimedElasticBandsParams& dto, const TimedElasticBandsParams& bo)
+ {
+ dto = bo.cfg;
+ }
+
+ void
+ fromAron(const arondto::TimedElasticBandsParams& dto, TimedElasticBandsParams& bo)
+ {
+ bo.cfg = dto;
+ }
+
+
+} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/aron_conversions.h b/source/armarx/navigation/local_planning/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b7d2382255a542a15fa9066a0a83348ec1e2fb2
--- /dev/null
+++ b/source/armarx/navigation/local_planning/aron_conversions.h
@@ -0,0 +1,22 @@
+#pragma once
+
+namespace armarx::navigation::local_planning
+{
+ // struct LocalPlannerParams;
+ struct TimedElasticBandsParams;
+
+ namespace arondto
+ {
+ struct LocalPlannerParams;
+ struct TimedElasticBandsParams;
+
+ } // namespace arondto
+
+} // namespace armarx::navigation::local_planning
+
+namespace armarx::navigation::local_planning
+{
+ void toAron(arondto::TimedElasticBandsParams& dto, const TimedElasticBandsParams& bo);
+ void fromAron(const arondto::TimedElasticBandsParams& dto, TimedElasticBandsParams& bo);
+
+} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/aron_conversions_teb.cpp b/source/armarx/navigation/local_planning/aron_conversions_teb.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d859dd9871ef97280221b33a3e932f08d7eb6449
--- /dev/null
+++ b/source/armarx/navigation/local_planning/aron_conversions_teb.cpp
@@ -0,0 +1,146 @@
+#include "aron_conversions.h"
+
+#include <teb_local_planner/teb_config.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+
+#include <armarx/navigation/local_planning/TimedElasticBands.h>
+#include <armarx/navigation/local_planning/aron/TimedElasticBands.aron.generated.h>
+
+namespace armarx::navigation::local_planning
+{
+
+ void
+ toTebCfg(const arondto::TimedElasticBandsParams& dto, ::teb_local_planner::TebConfig& bo)
+ {
+ 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;
+ bo.pse.lrk_enable_collision_check = dto.teb_config.pse.lrk_enable_collision_check;
+ bo.pse.hybrid_homotopy_calculation = dto.teb_config.pse.hybrid_homotopy_calculation;
+ bo.pse.robot_diff_circumscribed_inscribed_radius =
+ dto.teb_config.pse.robot_diff_circumscribed_inscribed_radius;
+
+ // Trajectory
+ bo.trajectory.teb_autosize = bo.trajectory.teb_autosize;
+ bo.trajectory.dt_ref = dto.teb_config.trajectory.dt_ref;
+ bo.trajectory.dt_hysteresis = dto.teb_config.trajectory.dt_hysteresis;
+ bo.trajectory.min_samples = dto.teb_config.trajectory.min_samples;
+ bo.trajectory.max_samples = dto.teb_config.trajectory.max_samples;
+ bo.trajectory.global_plan_overwrite_orientation =
+ dto.teb_config.trajectory.global_plan_overwrite_orientation;
+ bo.trajectory.allow_init_with_backwards_motion =
+ dto.teb_config.trajectory.allow_init_with_backwards_motion;
+ bo.trajectory.exact_arc_length = dto.teb_config.trajectory.exact_arc_length;
+ bo.trajectory.force_reinit_new_goal_dist =
+ dto.teb_config.trajectory.force_reinit_new_goal_dist;
+ bo.trajectory.force_reinit_new_goal_angular =
+ dto.teb_config.trajectory.force_reinit_new_goal_angular;
+ bo.trajectory.feasibility_check_no_poses =
+ dto.teb_config.trajectory.feasibility_check_no_poses;
+ bo.trajectory.feasibility_check_lookahead_distance =
+ dto.teb_config.trajectory.feasibility_check_lookahead_distance;
+ bo.trajectory.min_resolution_collision_check_angular =
+ dto.teb_config.trajectory.min_resolution_collision_check_angular;
+
+ // Robot
+ bo.robot.max_vel_x = dto.teb_config.robot.max_vel_x;
+ bo.robot.max_vel_x_backwards = dto.teb_config.robot.max_vel_x_backwards;
+ bo.robot.max_vel_y = dto.teb_config.robot.max_vel_y;
+ bo.robot.max_vel_trans = dto.teb_config.robot.max_vel_trans;
+ bo.robot.max_vel_theta = dto.teb_config.robot.max_vel_theta;
+ bo.robot.acc_lim_x = dto.teb_config.robot.acc_lim_x;
+ bo.robot.acc_lim_y = dto.teb_config.robot.acc_lim_y;
+ bo.robot.acc_lim_theta = dto.teb_config.robot.acc_lim_theta;
+ bo.robot.min_turning_radius = dto.teb_config.robot.min_turning_radius;
+
+ // GoalTolerance
+ bo.goal_tolerance.xy_goal_tolerance = dto.teb_config.goal_tolerance.xy_goal_tolerance;
+
+ // Obstacles
+ bo.obstacles.min_obstacle_dist = dto.teb_config.obstacles.min_obstacle_dist;
+ bo.obstacles.inflation_dist = dto.teb_config.obstacles.inflation_dist;
+ bo.obstacles.dynamic_obstacle_inflation_dist =
+ dto.teb_config.obstacles.dynamic_obstacle_inflation_dist;
+ bo.obstacles.include_dynamic_obstacles = dto.teb_config.obstacles.include_dynamic_obstacles;
+ bo.obstacles.obstacle_association_force_inclusion_factor =
+ dto.teb_config.obstacles.obstacle_association_force_inclusion_factor;
+ bo.obstacles.obstacle_association_cutoff_factor =
+ dto.teb_config.obstacles.obstacle_association_cutoff_factor;
+ bo.obstacles.obstacle_proximity_ratio_max_vel =
+ dto.teb_config.obstacles.obstacle_proximity_ratio_max_vel;
+ bo.obstacles.obstacle_proximity_lower_bound =
+ dto.teb_config.obstacles.obstacle_proximity_lower_bound;
+ bo.obstacles.obstacle_proximity_upper_bound =
+ dto.teb_config.obstacles.obstacle_proximity_upper_bound;
+
+ // Optimization
+ bo.optim.no_inner_iterations = dto.teb_config.optim.no_inner_iterations;
+ bo.optim.no_outer_iterations = dto.teb_config.optim.no_outer_iterations;
+ bo.optim.optimization_activate = dto.teb_config.optim.optimization_activate;
+ bo.optim.optimization_verbose = dto.teb_config.optim.optimization_verbose;
+ bo.optim.penalty_epsilon = dto.teb_config.optim.penalty_epsilon;
+ bo.optim.weight_max_vel_x = dto.teb_config.optim.weight_max_vel_x;
+ bo.optim.weight_max_vel_y = dto.teb_config.optim.weight_max_vel_y;
+ bo.optim.weight_max_vel_theta = dto.teb_config.optim.weight_max_vel_theta;
+ bo.optim.weight_acc_lim_x = dto.teb_config.optim.weight_acc_lim_x;
+ bo.optim.weight_acc_lim_y = dto.teb_config.optim.weight_acc_lim_y;
+ bo.optim.weight_acc_lim_theta = dto.teb_config.optim.weight_acc_lim_theta;
+ bo.optim.weight_kinematics_nh = dto.teb_config.optim.weight_kinematics_nh;
+ bo.optim.weight_kinematics_forward_drive =
+ dto.teb_config.optim.weight_kinematics_forward_drive;
+ bo.optim.weight_kinematics_turning_radius =
+ dto.teb_config.optim.weight_kinematics_turning_radius;
+ bo.optim.weight_optimaltime = dto.teb_config.optim.weight_optimaltime;
+ bo.optim.weight_shortest_path = dto.teb_config.optim.weight_shortest_path;
+ bo.optim.weight_obstacle = dto.teb_config.optim.weight_obstacle;
+ bo.optim.weight_inflation = dto.teb_config.optim.weight_inflation;
+ bo.optim.weight_dynamic_obstacle = dto.teb_config.optim.weight_dynamic_obstacle;
+ bo.optim.weight_dynamic_obstacle_inflation =
+ dto.teb_config.optim.weight_dynamic_obstacle_inflation;
+ bo.optim.weight_velocity_obstacle_ratio =
+ dto.teb_config.optim.weight_velocity_obstacle_ratio;
+ bo.optim.weight_viapoint = dto.teb_config.optim.weight_viapoint;
+ bo.optim.weight_prefer_rotdir = dto.teb_config.optim.weight_prefer_rotdir;
+ bo.optim.weight_adapt_factor = dto.teb_config.optim.weight_adapt_factor;
+ bo.optim.obstacle_cost_exponent = dto.teb_config.optim.obstacle_cost_exponent;
+
+ // Homotopy Class Planner
+ bo.hcp.enable_multithreading = dto.teb_config.hcp.enable_multithreading;
+ bo.hcp.simple_exploration = dto.teb_config.hcp.simple_exploration;
+ bo.hcp.max_number_classes = dto.teb_config.hcp.max_number_classes;
+ bo.hcp.max_number_plans_in_current_class =
+ dto.teb_config.hcp.max_number_plans_in_current_class;
+ bo.hcp.selection_cost_hysteresis = dto.teb_config.hcp.selection_cost_hysteresis;
+ bo.hcp.selection_obst_cost_scale = dto.teb_config.hcp.selection_obst_cost_scale;
+ bo.hcp.selection_viapoint_cost_scale = dto.teb_config.hcp.selection_viapoint_cost_scale;
+ bo.hcp.selection_alternative_time_cost = dto.teb_config.hcp.selection_alternative_time_cost;
+ bo.hcp.selection_dropping_probability = dto.teb_config.hcp.selection_dropping_probability;
+ bo.hcp.switching_blocking_period = dto.teb_config.hcp.switching_blocking_period;
+
+ bo.hcp.obstacle_heading_threshold = dto.teb_config.hcp.obstacle_heading_threshold;
+ bo.hcp.roadmap_graph_no_samples = dto.teb_config.hcp.roadmap_graph_no_samples;
+ bo.hcp.roadmap_graph_area_width = dto.teb_config.hcp.roadmap_graph_area_width;
+ bo.hcp.roadmap_graph_area_length_scale = dto.teb_config.hcp.roadmap_graph_area_length_scale;
+ bo.hcp.h_signature_prescaler = dto.teb_config.hcp.h_signature_prescaler;
+ bo.hcp.h_signature_threshold = dto.teb_config.hcp.h_signature_threshold;
+
+ bo.hcp.viapoints_all_candidates = dto.teb_config.hcp.viapoints_all_candidates;
+
+ bo.hcp.delete_detours_backwards = dto.teb_config.hcp.delete_detours_backwards;
+ bo.hcp.detours_orientation_tolerance = dto.teb_config.hcp.detours_orientation_tolerance;
+ bo.hcp.length_start_orientation_vector = dto.teb_config.hcp.length_start_orientation_vector;
+ bo.hcp.max_ratio_detours_duration_best_duration =
+ dto.teb_config.hcp.max_ratio_detours_duration_best_duration;
+
+ bo.checkParameters();
+ }
+
+
+} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/aron_conversions_teb.h b/source/armarx/navigation/local_planning/aron_conversions_teb.h
new file mode 100644
index 0000000000000000000000000000000000000000..4912045306ddc5b2ba48af8ec5ca83be24deca75
--- /dev/null
+++ b/source/armarx/navigation/local_planning/aron_conversions_teb.h
@@ -0,0 +1,19 @@
+#pragma once
+
+namespace teb_local_planner
+{
+ class TebConfig;
+}
+
+namespace armarx::navigation::local_planning::arondto
+{
+ struct TimedElasticBandsParams;
+
+} // namespace armarx::navigation::local_planning::arondto
+
+
+namespace armarx::navigation::local_planning
+{
+ void toTebCfg(const arondto::TimedElasticBandsParams& dto, teb_local_planner::TebConfig& bo);
+
+} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/core.h b/source/armarx/navigation/local_planning/core.h
index d2fa0f3e7a1a64ab67a204afafc0bce140fc1327..f8d1e1029449b7a8f57890c2652c93f1f943aea0 100644
--- a/source/armarx/navigation/local_planning/core.h
+++ b/source/armarx/navigation/local_planning/core.h
@@ -24,7 +24,7 @@
#include <SimoxUtility/meta/enum/EnumNames.hpp>
-namespace armarx::navigation::loc_plan
+namespace armarx::navigation::local_planning
{
enum class Algorithms
@@ -35,4 +35,4 @@ namespace armarx::navigation::loc_plan
const inline simox::meta::EnumNames<Algorithms> AlgorithmNames{
{Algorithms::TimedElasticBands, "TimedElasticBands"}};
-} // namespace armarx::navigation::loc_plan
+} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c348cfb1e0f1d6e37218d40435b12f7314fb1edf
--- /dev/null
+++ b/source/armarx/navigation/local_planning/ros_conversions.cpp
@@ -0,0 +1,209 @@
+#include "ros_conversions.h"
+
+#include <cmath>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/Duration.h"
+
+#include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/conversions/eigen.h>
+#include <range/v3/view/drop.hpp>
+#include <range/v3/view/zip.hpp>
+
+namespace armarx::navigation::conv
+{
+ Eigen::Vector2d
+ toRos2D(const Eigen::Vector2f& vec)
+ {
+ return vec.cast<double>() / 1000; // [mm] to [m]
+ }
+
+ Eigen::Vector2d
+ toRos(const Eigen::Vector3f& vec)
+ {
+ return conv::to2D(vec).cast<double>() / 1000; // [mm] to [m]
+ }
+
+ Eigen::Vector3f
+ fromRos(const Eigen::Vector2d& vec)
+ {
+ return conv::to3D(vec.cast<float>()) * 1000; // [m] to [mm]
+ }
+
+
+ teb_local_planner::PoseSE2
+ toRos(const core::Pose& pose)
+ {
+ core::Pose2D pose2D = to2D(pose);
+ return toRos(pose2D);
+ }
+
+ teb_local_planner::PoseSE2
+ toRos(const core::Pose2D& pose)
+ {
+ Eigen::Vector2d pos = pose.translation().cast<double>();
+ double theta = Eigen::Rotation2Df(pose.linear()).angle();
+
+ // 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]
+ }
+
+
+ core::Pose
+ fromRos(const teb_local_planner::PoseSE2& pose)
+ {
+ Eigen::Vector2d pos = pose.position() * 1000; // [m] to [mm]
+ double theta = pose.theta();
+
+ // we change it such that x is pointing forward.
+ // ROS: x pointing forward, ArmarX: y pointing forward
+ theta -= M_PI_2;
+
+
+ core::Pose2D ret;
+ ret.translation() = pos.cast<float>();
+ ret.linear() = Eigen::Rotation2Df(theta).toRotationMatrix();
+
+ return to3D(ret);
+ }
+
+ geometry_msgs::Twist
+ toRos(const core::Twist& velocity)
+ {
+ geometry_msgs::Twist ret;
+
+ // we change it such that x is pointing forward.
+ // ROS: x pointing forward, ArmarX: y pointing forward
+
+ ret.linear.x = velocity.linear.y() / 1000; // [mm] to [m]
+ ret.linear.y = -velocity.linear.x() / 1000; // [mm] to [m]
+ ret.angular.z = velocity.angular.z();
+
+ return ret;
+ }
+
+ teb_local_planner::TimedElasticBand
+ toRos(const core::GlobalTrajectory& trajectory)
+ {
+ teb_local_planner::TimedElasticBand teb;
+
+ bool first = true;
+ teb_local_planner::PoseSE2 lastPose;
+ for (const core::GlobalTrajectoryPoint& point : trajectory.points())
+ {
+ teb_local_planner::PoseSE2 pose = toRos(point.waypoint.pose);
+ teb.addPose(pose);
+
+ if (!first)
+ {
+ Eigen::Vector2d distance = pose.position() - lastPose.position();
+ double timeDiff = distance.norm() / (point.velocity / 1000); // [mm/s] -> [m/s]
+ teb.addTimeDiff(timeDiff);
+
+ first = false;
+ }
+
+ lastPose = pose;
+ }
+
+ return teb;
+ }
+
+ core::LocalTrajectory
+ fromRos(const teb_local_planner::TimedElasticBand& teb)
+ {
+ const auto& tebPoses = teb.poses();
+
+ if (tebPoses.empty())
+ {
+ return {};
+ }
+
+ core::LocalTrajectoryPoints trajectory;
+ trajectory.reserve(tebPoses.size());
+
+ // TODO this timestamp should be given via the argument list
+ DateTime timestamp = Clock::Now();
+
+ ARMARX_CHECK_EQUAL(tebPoses.size() - 1, teb.timediffs().size());
+
+ // add the first pose at this timestamp
+ trajectory.push_back(core::LocalTrajectoryPoint{
+ .timestamp = timestamp, .pose = {fromRos(tebPoses.front()->pose())}});
+
+ // all consecutive poses
+ for (const auto& [poseVertex, timediff] :
+ ranges::views::zip(tebPoses | ranges::views::drop(1), teb.timediffs()))
+ {
+ const core::Pose pose = fromRos(poseVertex->pose());
+ const Duration dt = Duration::SecondsDouble(timediff->dt());
+
+ timestamp += dt;
+ trajectory.push_back(
+ core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {pose}});
+ }
+
+ ARMARX_CHECK_EQUAL(trajectory.size(), tebPoses.size());
+
+ return {trajectory};
+ }
+
+ human::shapes::Ellipse
+ toRos(const human::shapes::Ellipse& ellipse)
+ {
+ // a,b parameter are along the x and y axis respectively
+ // ROS and ArmarX have the x axis pointing in a different direction
+ // ROS: x pointing forward, ArmarX: y pointing forward
+ return {.a = ellipse.b / 1000, .b = ellipse.a / 1000}; // [mm] to [m]
+ }
+
+ teb_local_planner::Costmap
+ toRos(const algorithms::Costmap& costmap)
+ {
+ const algorithms::Costmap::Parameters& params = costmap.params();
+ const teb_local_planner::Costmap::Parameters teb_params = {
+ params.binaryGrid, params.cellSize / 1000}; // [mm] to [m]
+
+ const algorithms::SceneBounds& bounds = costmap.getLocalSceneBounds();
+ const teb_local_planner::Costmap::SceneBounds teb_bounds = {
+ toRos2D(bounds.min).cast<float>(), toRos2D(bounds.max).cast<float>()};
+
+ const std::optional<algorithms::Costmap::Mask>& mask = costmap.getMask();
+ boost::optional<teb_local_planner::Costmap::Mask> teb_mask = boost::none;
+ if (mask)
+ {
+ teb_mask = mask.value();
+ }
+
+ teb_local_planner::Costmap::Pose2D teb_origin = costmap.origin();
+ teb_origin.translation() /= 1000; // [mm] to[m]
+
+ algorithms::Costmap::Grid teb_grid = costmap.getGrid();
+ teb_grid /= 1000; // [mm] to [m]
+
+ return teb_local_planner::Costmap{teb_grid, teb_params, teb_bounds, teb_mask, teb_origin};
+ }
+
+ teb_local_planner::PenaltyModelPtr
+ toRos(const human::LinearPenaltyModel& model)
+ {
+ return boost::make_shared<teb_local_planner::LinearPenaltyModel>(
+ teb_local_planner::LinearPenaltyModel(model.minDistance, model.epsilon));
+ }
+
+ teb_local_planner::PenaltyModelPtr
+ toRos(const human::ExponentialPenaltyModel& model)
+ {
+ return boost::make_shared<teb_local_planner::ExponentialPenaltyModel>(
+ teb_local_planner::LinearPenaltyModel(model.minDistance, model.epsilon),
+ model.exponent);
+ }
+
+} // namespace armarx::navigation::conv
diff --git a/source/armarx/navigation/local_planning/ros_conversions.h b/source/armarx/navigation/local_planning/ros_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..083ddcc57b2e78074c06428ce31acaf06e343514
--- /dev/null
+++ b/source/armarx/navigation/local_planning/ros_conversions.h
@@ -0,0 +1,61 @@
+/**
+ * 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 )
+ * @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/basic_types.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/human/shapes.h>
+#include <geometry_msgs/Twist.h>
+#include <teb_local_planner/costmap.h>
+#include <teb_local_planner/penalty_models.h>
+#include <teb_local_planner/pose_se2.h>
+#include <teb_local_planner/timed_elastic_band.h>
+
+namespace armarx::navigation::conv
+{
+
+ Eigen::Vector2d toRos2D(const Eigen::Vector2f& vec);
+
+ Eigen::Vector2d toRos(const Eigen::Vector3f& vec);
+ Eigen::Vector3f fromRos(const Eigen::Vector2d& vec);
+
+ teb_local_planner::PoseSE2 toRos(const core::Pose& pose);
+ teb_local_planner::PoseSE2 toRos(const core::Pose2D& pose);
+
+ core::Pose fromRos(const teb_local_planner::PoseSE2& pose);
+
+ geometry_msgs::Twist toRos(const core::Twist& velocity);
+
+ teb_local_planner::TimedElasticBand toRos(const core::GlobalTrajectory& trajectory);
+
+ core::LocalTrajectory fromRos(const teb_local_planner::TimedElasticBand& teb);
+
+ human::shapes::Ellipse toRos(const human::shapes::Ellipse& ellipse);
+
+ teb_local_planner::Costmap toRos(const algorithms::Costmap& costmap);
+
+ teb_local_planner::PenaltyModelPtr toRos(const human::LinearPenaltyModel& model);
+
+ teb_local_planner::PenaltyModelPtr toRos(const human::ExponentialPenaltyModel& model);
+
+} // namespace armarx::navigation::conv
diff --git a/source/armarx/navigation/memory/CMakeLists.txt b/source/armarx/navigation/memory/CMakeLists.txt
index f2d52dd1dbd5c524db1bacba84bc51c1e53c4aec..c03f9a2b8fb622325659fd65c4ac9016fd22bfaf 100644
--- a/source/armarx/navigation/memory/CMakeLists.txt
+++ b/source/armarx/navigation/memory/CMakeLists.txt
@@ -2,23 +2,29 @@ armarx_add_library(memory
SOURCES
#./memory.cpp
client/stack_result/Writer.cpp
+ client/stack_result/Reader.cpp
client/parameterization/Reader.cpp
client/parameterization/Writer.cpp
client/graph/Reader.cpp
client/events/Writer.cpp
client/costmap/Writer.cpp
client/costmap/Reader.cpp
+ client/human/Reader.cpp
+ client/human/Writer.cpp
client/rooms/Reader.cpp
# ./client/events/Reader.cpp
HEADERS
memory.h
client/stack_result/Writer.h
+ client/stack_result/Reader.h
client/parameterization/Reader.h
client/parameterization/Writer.h
client/graph/Reader.h
client/events/Writer.h
client/costmap/Writer.h
client/costmap/Reader.h
+ client/human/Reader.h
+ client/human/Writer.h
client/rooms/Reader.h
# ./client/events/Reader.h
DEPENDENCIES
@@ -29,6 +35,7 @@ armarx_add_library(memory
armarx_navigation::algorithms
armarx_navigation::graph
armarx_navigation::location
+ armarx_navigation::teb_human
armarx_navigation::rooms
)
diff --git a/source/armarx/navigation/memory/client/costmap/Reader.cpp b/source/armarx/navigation/memory/client/costmap/Reader.cpp
index 436fd2b9462f5a3132607f7302315443ccb4e5eb..ef078b77da2b9dfa6e1e7425c1fabf15606c98e8 100644
--- a/source/armarx/navigation/memory/client/costmap/Reader.cpp
+++ b/source/armarx/navigation/memory/client/costmap/Reader.cpp
@@ -96,7 +96,7 @@ namespace armarx::navigation::memory::client::costmap
if (not coreSegment.hasProviderSegment(query.providerName))
{
- ARMARX_WARNING << "Provider segment `" << query.providerName
+ ARMARX_VERBOSE << "Provider segment `" << query.providerName
<< "` does not exist (yet).";
return {.costmap = std::nullopt, .status = Result::Status::NoData};
}
@@ -106,7 +106,7 @@ namespace armarx::navigation::memory::client::costmap
if (providerSegment.empty())
{
- ARMARX_WARNING << "No entities.";
+ ARMARX_VERBOSE << "No entities.";
return {.costmap = std::nullopt,
.status = Result::Status::NoData,
.errorMessage = "No entities"};
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/memory/client/graph/Reader.cpp b/source/armarx/navigation/memory/client/graph/Reader.cpp
index 51a9b890c30a73ed0cddc2ed66a84cba80cfae36..bb55fc7a4b70e8bb25c884ad1996e47797be0d6a 100644
--- a/source/armarx/navigation/memory/client/graph/Reader.cpp
+++ b/source/armarx/navigation/memory/client/graph/Reader.cpp
@@ -41,7 +41,9 @@ namespace armarx::navigation::memory::client::graph
const auto locs = queryLocations();
for (const auto& [id, location] : locs)
{
- locations.emplace(id.entityName, core::Pose(location.globalRobotPose));
+ const std::string locationName = id.providerSegmentName + "/" + id.entityName;
+ ARMARX_DEBUG << "Location: " << locationName;
+ locations.emplace(locationName, core::Pose(location.globalRobotPose));
}
return locations;
diff --git a/source/armarx/navigation/memory/client/human/Reader.cpp b/source/armarx/navigation/memory/client/human/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5b3398abc3a5c3de3ee2f1b58edf743686c1d8b
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Reader.cpp
@@ -0,0 +1,250 @@
+#include "Reader.h"
+
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include <armarx/navigation/human/types.h>
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
+
+namespace armarx::navigation::memory::client::human
+{
+ Reader::~Reader() = default;
+
+ armarx::armem::client::query::Builder
+ Reader::buildHumansQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties().coreSegmentName)
+ .providerSegments().withName(query.providerName)
+ .entities().withName("humans")
+ .snapshots().beforeOrAtTime(query.timestamp);
+ // clang-format on
+
+ return qb;
+ }
+
+ armarx::armem::client::query::Builder
+ Reader::buildHumanGroupsQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties().coreSegmentName)
+ .providerSegments().withName(query.providerName)
+ .entities().withName("groups")
+ .snapshots().beforeOrAtTime(query.timestamp);
+ // clang-format on
+
+ return qb;
+ }
+
+
+ std::string
+ Reader::propertyPrefix() const
+ {
+ return "mem.nav.human.";
+ }
+
+ armarx::armem::client::util::SimpleReaderBase::Properties
+ Reader::defaultProperties() const
+ {
+ return {.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName = memory::constants::HumanCoreSegmentName};
+ }
+
+ navigation::human::Humans
+ asHumans(const armem::wm::ProviderSegment& providerSegment,
+ const DateTime& timestamp,
+ const Duration& maxAge)
+ {
+ navigation::human::Humans humans;
+
+ ARMARX_CHECK(not providerSegment.empty()) << "No entities";
+ ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+
+ providerSegment.forEachEntity(
+ [&humans, ×tamp, &maxAge](const armem::wm::Entity& entity)
+ {
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ entitySnapshot.forEachInstance(
+ [&](const armem::wm::EntityInstance& entityInstance)
+ {
+ const Duration dtToNow = timestamp - entityInstance.metadata().timeCreated;
+
+ if (dtToNow < maxAge and dtToNow.isPositive())
+ {
+ const auto dto =
+ navigation::human::arondto::Human::FromAron(entityInstance.data());
+
+ navigation::human::Human human;
+ fromAron(dto, human);
+ humans.push_back(human);
+ }
+ });
+ });
+
+ return humans;
+ }
+
+ navigation::human::HumanGroups
+ asGroups(const armem::wm::ProviderSegment& providerSegment,
+ const DateTime& timestamp,
+ const Duration& maxAge)
+ {
+ navigation::human::HumanGroups humans;
+
+ ARMARX_CHECK(not providerSegment.empty()) << "No entities";
+ ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+
+ providerSegment.forEachEntity(
+ [&humans, ×tamp, &maxAge](const armem::wm::Entity& entity)
+ {
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ entitySnapshot.forEachInstance(
+ [&](const armem::wm::EntityInstance& entityInstance)
+ {
+ const Duration dtToNow = timestamp - entityInstance.metadata().timeCreated;
+
+ if (dtToNow < maxAge and dtToNow.isPositive())
+ {
+ const auto dto = navigation::human::arondto::HumanGroup::FromAron(
+ entityInstance.data());
+
+ navigation::human::HumanGroup human;
+ fromAron(dto, human);
+ humans.push_back(human);
+ }
+ });
+ });
+
+ return humans;
+ }
+
+ Reader::HumanGroupResult
+ Reader::queryHumanGroups(const Query& query) const
+ {
+ const auto qb = buildHumansQuery(query);
+
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
+
+ const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.groups = {},
+ .status = HumanGroupResult::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+ if (not coreSegment.hasProviderSegment(query.providerName))
+ {
+ ARMARX_DEBUG << "Provider segment `" << query.providerName << "` does not exist (yet).";
+ return {.groups = {}, .status = HumanGroupResult::Status::NoData};
+ }
+
+ const armem::wm::ProviderSegment& providerSegment =
+ coreSegment.getProviderSegment(query.providerName);
+
+ if (providerSegment.empty())
+ {
+ ARMARX_DEBUG << "No entities.";
+ return {.groups = {},
+ .status = HumanGroupResult::Status::NoData,
+ .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ return HumanGroupResult{.groups =
+ asGroups(providerSegment, query.timestamp, query.maxAge),
+ .status = HumanGroupResult::Status::Success};
+ }
+ catch (...)
+ {
+ return HumanGroupResult{.groups = {},
+ .status = HumanGroupResult::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+
+ Reader::HumanResult
+ Reader::queryHumans(const Query& query) const
+ {
+ const auto qb = buildHumansQuery(query);
+
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
+
+ const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.humans = {},
+ .status = HumanResult::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+ if (not coreSegment.hasProviderSegment(query.providerName))
+ {
+ ARMARX_VERBOSE << "Provider segment `" << query.providerName
+ << "` does not exist (yet).";
+ return {.humans = {}, .status = HumanResult::Status::NoData};
+ }
+
+ const armem::wm::ProviderSegment& providerSegment =
+ coreSegment.getProviderSegment(query.providerName);
+
+ if (providerSegment.empty())
+ {
+ ARMARX_VERBOSE << "No entities.";
+ return {
+ .humans = {}, .status = HumanResult::Status::NoData, .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ return HumanResult{.humans = asHumans(providerSegment, query.timestamp, query.maxAge),
+ .status = HumanResult::Status::Success};
+ }
+ catch (...)
+ {
+ return HumanResult{.humans = {},
+ .status = HumanResult::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/client/human/Reader.h b/source/armarx/navigation/memory/client/human/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..d6dbaf74c55e7be32cfc8e7772368aaf4b8cfa16
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Reader.h
@@ -0,0 +1,98 @@
+/*
+ * 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 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include <armarx/navigation/human/types.h>
+#include <armarx/navigation/algorithms/Costmap.h>
+
+namespace armarx::navigation::memory::client::human
+{
+
+ class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+ ~Reader() override;
+
+ struct Query
+ {
+ std::string providerName;
+ armem::Time timestamp;
+ Duration maxAge;
+ };
+
+ struct HumanResult
+ {
+ armarx::navigation::human::Humans humans;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ struct HumanGroupResult
+ {
+ armarx::navigation::human::HumanGroups groups;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ HumanResult queryHumans(const Query& query) const;
+ HumanGroupResult queryHumanGroups(const Query& query) const;
+
+ protected:
+ ::armarx::armem::client::query::Builder buildHumansQuery(const Query& query) const;
+ ::armarx::armem::client::query::Builder buildHumanGroupsQuery(const Query& query) const;
+
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/client/human/Writer.cpp b/source/armarx/navigation/memory/client/human/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ef31d0aa8396bea87c1f45995be23f5bc70274b9
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Writer.cpp
@@ -0,0 +1,133 @@
+#include "Writer.h"
+
+#include <iterator>
+
+#include <armarx/navigation/human/types.h>
+#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/memory/constants.h>
+#include <armarx/navigation/human/aron_conversions.h>
+
+
+namespace armarx::navigation::memory::client::human
+{
+ Writer::~Writer() = default;
+
+ bool
+ Writer::store(const armarx::navigation::human::Humans& humans,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex()};
+
+ const auto result =
+ memoryWriter().addSegment(memory::constants::HumanCoreSegmentName, providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID = providerId.withEntityName("humans").withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ std::transform(humans.begin(),
+ humans.end(),
+ std::back_inserter(update.instancesData),
+ [](const navigation::human::Human& human) -> armarx::aron::data::DictPtr {
+ navigation::human::arondto::Human dto;
+ toAron(dto, human);
+
+ return dto.toAron();
+ });
+
+
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+ bool
+ Writer::store(const armarx::navigation::human::HumanGroups& groups,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex()};
+
+ const auto result =
+ memoryWriter().addSegment(memory::constants::HumanCoreSegmentName, providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID = providerId.withEntityName("groups").withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ std::transform(groups.begin(),
+ groups.end(),
+ std::back_inserter(update.instancesData),
+ [](const navigation::human::HumanGroup& group) -> armarx::aron::data::DictPtr {
+ navigation::human::arondto::HumanGroup dto;
+ toAron(dto, group);
+
+ return dto.toAron();
+ });
+
+
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+ std::string
+ Writer::propertyPrefix() const
+ {
+ return "mem.nav.human.";
+ }
+
+ armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+ Writer::defaultProperties() const
+ {
+ return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName =
+ memory::constants::HumanCoreSegmentName};
+ }
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/client/human/Writer.h b/source/armarx/navigation/memory/client/human/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..b224149c4aeb3c4858530f19ed051700060fc179
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Writer.h
@@ -0,0 +1,69 @@
+/*
+ * 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 RobotAPI::ArmarXObjects::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::memory::client::human
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class Writer : virtual public armarx::armem::client::util::SimpleWriterBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase;
+ ~Writer() override;
+
+ bool store(const armarx::navigation::human::Humans& humans,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp);
+
+ bool store(const armarx::navigation::human::HumanGroups& groups,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp);
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/client/rooms/Reader.cpp b/source/armarx/navigation/memory/client/rooms/Reader.cpp
index a3531ad9fd78c2842ede4f55d07b219b5896ac7f..c89bd4c037aeafea8be8c383379d733029ab068d 100644
--- a/source/armarx/navigation/memory/client/rooms/Reader.cpp
+++ b/source/armarx/navigation/memory/client/rooms/Reader.cpp
@@ -31,9 +31,9 @@ namespace armarx::navigation::memory::client::rooms
// clang-format off
qb
- .coreSegments().withName(properties().coreSegmentName)
+ .coreSegments().withName(memory::constants::RoomsCoreSegmentName)
.providerSegments().withName(query.providerName)
- .entities().all()
+ .entities().withName(query.name)
.snapshots().beforeOrAtTime(query.timestamp);
// clang-format on
@@ -57,7 +57,7 @@ namespace armarx::navigation::memory::client::rooms
asRoom(const armem::wm::ProviderSegment& providerSegment)
{
ARMARX_CHECK(not providerSegment.empty()) << "No entities";
- ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+ ARMARX_CHECK_EQUAL(providerSegment.size(), 1) << "There should be only one entity!";
const armem::wm::EntityInstance* entityInstance = nullptr;
providerSegment.forEachEntity(
@@ -72,7 +72,7 @@ namespace armarx::navigation::memory::client::rooms
ARMARX_CHECK_NOT_NULL(entityInstance);
const auto aronDto = armem::tryCast<navigation::rooms::arondto::Room>(*entityInstance);
- ARMARX_CHECK(aronDto) << "Failed casting to Costmap";
+ ARMARX_CHECK(aronDto) << "Failed casting to Room";
const navigation::rooms::arondto::Room& dto = *aronDto;
navigation::rooms::Room room;
@@ -86,11 +86,11 @@ namespace armarx::navigation::memory::client::rooms
{
const auto qb = buildQuery(query);
- ARMARX_DEBUG << "[MappingDataReader] query ... ";
+ ARMARX_DEBUG << "[rooms::Reader] query ... ";
const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
- ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+ ARMARX_DEBUG << "[rooms::Reader] result: " << qResult;
if (not qResult.success)
{
diff --git a/source/armarx/navigation/memory/client/stack_result/Reader.cpp b/source/armarx/navigation/memory/client/stack_result/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f95e199747e1419c0c439d6fed1e287936d31de0
--- /dev/null
+++ b/source/armarx/navigation/memory/client/stack_result/Reader.cpp
@@ -0,0 +1,120 @@
+#include "Reader.h"
+#include "RobotAPI/libraries/armem/client/query/Builder.h"
+
+#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
+#include <armarx/navigation/core/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
+
+namespace armarx::navigation::memory::client::stack_result
+{
+ armarx::navigation::memory::client::stack_result::Reader::LocalPlannerResult
+ Reader::queryLocalPlannerResult(const Query& query)
+ {
+ const auto qb = buildLocalPlannerResultQuery(query);
+ const auto& providerName = query.clientID;
+
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
+
+ const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.trajectory = {},
+ .status = LocalPlannerResult::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+ if (not coreSegment.hasProviderSegment(providerName))
+ {
+ ARMARX_DEBUG << "Provider segment `" << providerName << "` does not exist (yet).";
+ return {.trajectory = {}, .status = LocalPlannerResult::Status::NoData};
+ }
+
+ const armem::wm::ProviderSegment& providerSegment =
+ coreSegment.getProviderSegment(providerName);
+
+ if (providerSegment.empty())
+ {
+ ARMARX_DEBUG << "No entities.";
+ return {.trajectory = {},
+ .status = LocalPlannerResult::Status::NoData,
+ .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ return LocalPlannerResult{.trajectory = asLocalPlannerResult(providerSegment),
+ .status = LocalPlannerResult::Status::Success};
+ }
+ catch (...)
+ {
+ return LocalPlannerResult{.trajectory = {},
+ .status = LocalPlannerResult::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+
+ std::string
+ Reader::propertyPrefix() const
+ {
+ return "mem.nav.stack_result.";
+ }
+
+ Reader::Properties
+ Reader::defaultProperties() const
+ {
+ return Properties{.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName = memory::constants::LocalPlannerResultCoreSegment};
+ }
+
+ armarx::navigation::core::LocalTrajectory
+ Reader::asLocalPlannerResult(const armem::wm::ProviderSegment& providerSegment)
+ {
+ navigation::human::HumanGroups humans;
+
+ ARMARX_CHECK(not providerSegment.empty()) << "No entities";
+ ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+
+ const armem::wm::EntityInstance* entityInstance = nullptr;
+
+ providerSegment.forEachEntity(
+ [&entityInstance](const armem::wm::Entity& entity)
+ {
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ entityInstance = &entitySnapshot.getInstance(0);
+ });
+
+ ARMARX_CHECK_NOT_NULL(entityInstance);
+
+ const auto dto =
+ navigation::core::arondto::LocalTrajectory::FromAron(entityInstance->data());
+
+ armarx::navigation::core::LocalTrajectory localTrajectory;
+ fromAron(dto, localTrajectory);
+
+ return localTrajectory;
+ }
+
+ armarx::armem::client::query::Builder Reader::buildLocalPlannerResultQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties().coreSegmentName)
+ .providerSegments().withName(query.clientID)
+ .entities().withName("trajectory")
+ .snapshots().beforeOrAtTime(query.timestamp);
+ // clang-format on
+
+ return qb;
+ }
+
+} // namespace armarx::navigation::memory::client::stack_result
diff --git a/source/armarx/navigation/memory/client/stack_result/Reader.h b/source/armarx/navigation/memory/client/stack_result/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..071ffe7acab9fb15610961160de5852286a577bd
--- /dev/null
+++ b/source/armarx/navigation/memory/client/stack_result/Reader.h
@@ -0,0 +1,104 @@
+/**
+ * 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 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "ArmarXCore/core/time/DateTime.h"
+
+#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
+
+#include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/local_planning/LocalPlanner.h>
+
+namespace armarx::navigation::memory::client::stack_result
+{
+
+ class Reader : virtual public armem::client::util::SimpleReaderBase
+ {
+ public:
+ using armem::client::util::SimpleReaderBase::SimpleReaderBase;
+
+ struct Query
+ {
+ DateTime timestamp;
+ std::string clientID;
+ };
+
+
+ struct LocalPlannerResult
+ {
+ armarx::navigation::core::LocalTrajectory trajectory;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ struct GlobalPlannerResult
+ {
+ armarx::navigation::core::GlobalTrajectory trajectory;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ GlobalPlannerResult queryGlobalPlannerResult(const Query& query);
+ LocalPlannerResult queryLocalPlannerResult(const Query& query);
+
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+
+ protected:
+ private:
+ armarx::navigation::core::LocalTrajectory
+ asLocalPlannerResult(const armem::wm::ProviderSegment& providerSegment);
+
+ armarx::navigation::core::GlobalTrajectory
+ asGlobalPlannerResult(const armem::wm::ProviderSegment& providerSegment);
+
+ armarx::armem::client::query::Builder
+ buildLocalPlannerResultQuery(const Query& query) const;
+
+ armarx::armem::client::query::Builder
+ buildGlobalPlannerResultQuery(const Query& query) const;
+ };
+} // namespace armarx::navigation::memory::client::stack_result
diff --git a/source/armarx/navigation/memory/client/stack_result/Writer.cpp b/source/armarx/navigation/memory/client/stack_result/Writer.cpp
index 759b42f6aadb4241566b9a4e412c912a13b436bf..1fbdf8e14bbb528590a9698e67e93286442f5cb9 100644
--- a/source/armarx/navigation/memory/client/stack_result/Writer.cpp
+++ b/source/armarx/navigation/memory/client/stack_result/Writer.cpp
@@ -4,9 +4,11 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
#include <armarx/navigation/core/aron/Twist.aron.generated.h>
#include <armarx/navigation/core/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::stack_result
{
@@ -17,8 +19,8 @@ namespace armarx::navigation::memory::client::stack_result
armem::Commit commit;
storePrepare(result.globalPlan, clientID, commit);
- // FIXME storePrepare(result.localTrajectory, clientID, commit);
- storePrepare(result.controlVelocity, clientID, commit);
+ storePrepare(result.localTrajectory, clientID, commit);
+ // storePrepare(result.controlVelocity, clientID, commit);
// FIXME storePrepare(result.safeVelocity, clientID, commit);
std::lock_guard g{memoryWriterMutex()};
@@ -45,7 +47,7 @@ namespace armarx::navigation::memory::client::stack_result
}
bool
- Writer::store(const armarx::navigation::traj_ctrl::TrajectoryControllerResult& result,
+ Writer::store(const armarx::navigation::local_planning::LocalPlannerResult& result,
const std::string& clientID)
{
armem::Commit commit;
@@ -55,6 +57,7 @@ namespace armarx::navigation::memory::client::stack_result
return updateResult.allSuccess();
}
+
bool
Writer::storePrepare(const armarx::navigation::global_planning::GlobalPlannerResult& result,
const std::string& clientID,
@@ -66,13 +69,13 @@ namespace armarx::navigation::memory::client::stack_result
armem::EntityUpdate update;
update.entityID = armem::MemoryID()
.withMemoryName(properties().memoryName)
- .withCoreSegmentName("Results_GlobalPlanner")
+ .withCoreSegmentName(constants::GlobalPlannerResultCoreSegment)
.withProviderSegmentName(clientID)
.withEntityName("trajectory")
.withTimestamp(timestamp);
- core::arondto::Trajectory aronDto;
+ core::arondto::GlobalTrajectory aronDto;
// FIXME create Aron type
core::toAron(aronDto, result.trajectory);
@@ -84,8 +87,9 @@ namespace armarx::navigation::memory::client::stack_result
return true;
}
+
bool
- Writer::storePrepare(const armarx::navigation::traj_ctrl::TrajectoryControllerResult& result,
+ Writer::storePrepare(const local_planning::LocalPlannerResult& result,
const std::string& clientID,
armem::Commit& commit)
{
@@ -95,15 +99,15 @@ namespace armarx::navigation::memory::client::stack_result
armem::EntityUpdate update;
update.entityID = armem::MemoryID()
.withMemoryName(properties().memoryName)
- .withCoreSegmentName("Results_TrajectoryController")
+ .withCoreSegmentName(constants::LocalPlannerResultCoreSegment)
.withProviderSegmentName(clientID)
- .withEntityName("velocity")
+ .withEntityName("trajectory")
.withTimestamp(timestamp);
- core::arondto::Twist aronDto;
- // FIXME own type
- core::toAron(aronDto, result.twist);
+ core::arondto::LocalTrajectory aronDto;
+ // FIXME create Aron type
+ toAron(aronDto, result.trajectory);
update.timeCreated = timestamp;
update.instancesData = {aronDto.toAron()};
@@ -113,6 +117,7 @@ namespace armarx::navigation::memory::client::stack_result
return true;
}
+
std::string
Writer::propertyPrefix() const
{
diff --git a/source/armarx/navigation/memory/client/stack_result/Writer.h b/source/armarx/navigation/memory/client/stack_result/Writer.h
index 1ece55943f1c3bceeecaf6d7385918df78866412..81468a7f5621b9733c5df7312bc3dbdf48c057df 100644
--- a/source/armarx/navigation/memory/client/stack_result/Writer.h
+++ b/source/armarx/navigation/memory/client/stack_result/Writer.h
@@ -24,9 +24,10 @@
#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
#include <RobotAPI/libraries/armem/core/Commit.h>
+#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/server/StackResult.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::memory::client::stack_result
{
@@ -40,7 +41,8 @@ namespace armarx::navigation::memory::client::stack_result
bool store(const armarx::navigation::global_planning::GlobalPlannerResult& result,
const std::string& clientID);
- bool store(const armarx::navigation::traj_ctrl::TrajectoryControllerResult& result,
+
+ bool store(const armarx::navigation::local_planning::LocalPlannerResult& result,
const std::string& clientID);
@@ -49,18 +51,10 @@ namespace armarx::navigation::memory::client::stack_result
const std::string& clientID,
armem::Commit& commit);
- // bool storePrepare(const server::StackResult::LocalPlannerResult& result,
- // const std::string& clientID,
- // armem::Commit& commit);
-
- bool storePrepare(const armarx::navigation::traj_ctrl::TrajectoryControllerResult& result,
+ bool storePrepare(const local_planning::LocalPlannerResult& result,
const std::string& clientID,
armem::Commit& commit);
- // bool storePrepare(const server::StackResult::SafetyControllerResult& result,
- // const std::string& clientID,
- // armem::Commit& commit);
-
protected:
std::string propertyPrefix() const override;
Properties defaultProperties() const override;
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index e81c8db6abf528af44466f129141aedebdb65879..177f9730e047da14810c1d7ad5dcad2be60ef7ab 100644
--- a/source/armarx/navigation/memory/constants.h
+++ b/source/armarx/navigation/memory/constants.h
@@ -32,6 +32,10 @@ namespace armarx::navigation::memory::constants
inline const std::string GraphCoreSegmentName = "Graph";
inline const std::string LocationCoreSegmentName = "Location";
inline const std::string CostmapCoreSegmentName = "Costmap";
+ inline const std::string HumanCoreSegmentName = "Human";
+
+ inline const std::string GlobalPlannerResultCoreSegment = "Results_GlobalPlanner";
+ inline const std::string LocalPlannerResultCoreSegment = "Results_LocalPlanner";
inline const std::string RoomsCoreSegmentName = "Rooms";
} // namespace armarx::navigation::memory::constants
diff --git a/source/armarx/navigation/platform_controller/CMakeLists.txt b/source/armarx/navigation/platform_controller/CMakeLists.txt
index 509c8f9af074a17329a3a9ba0282f7a411370dcd..78b24397fa54b5a8fc85a32c883395833e462d9c 100644
--- a/source/armarx/navigation/platform_controller/CMakeLists.txt
+++ b/source/armarx/navigation/platform_controller/CMakeLists.txt
@@ -1,24 +1,23 @@
armarx_add_aron_library(platform_controller_aron
ARON_FILES
- aron/PlatformTrajectoryControllerConfig.xml
+ aron/PlatformLocalTrajectoryControllerConfig.xml
+ aron/PlatformGlobalTrajectoryControllerConfig.xml
DEPENDENCIES
armarx_control::common_aron
)
armarx_add_library(platform_controller
SOURCES
- # WholeBodyImpedanceController.cpp
- PlatformTrajectoryController.cpp
+ PlatformLocalTrajectoryController.cpp
+ PlatformGlobalTrajectoryController.cpp
aron_conversions.cpp
HEADERS
- # WholeBodyImpedanceController.h
- PlatformTrajectoryController.h
+ PlatformLocalTrajectoryController.h
+ PlatformGlobalTrajectoryController.h
aron_conversions.h
DEPENDENCIES_PUBLIC
Simox::VirtualRobot
- armarx_control::common
armarx_control::client
armarx_navigation::core
armarx_navigation::trajectory_control
- # armarx_control::njoint_qp_controller_aron
)
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.cpp
similarity index 89%
rename from source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
rename to source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.cpp
index b5da2a9095d386ece1ec7189a31e7a4c5ef04efc..7ad11344516a7de794802bd69eee7ba968eeb690 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
+++ b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.cpp
@@ -1,4 +1,4 @@
-#include "PlatformTrajectoryController.h"
+#include "PlatformGlobalTrajectoryController.h"
#include "ArmarXCore/core/ArmarXObjectScheduler.h"
#include "ArmarXCore/core/logging/Logging.h"
@@ -7,17 +7,15 @@
#include "RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h"
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
-#include <armarx/control/common/aron_conversions.h>
#include <armarx/control/common/type.h>
-// #include <armarx/control/common/utils.h>
#include <armarx/navigation/common/controller_types.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/aron_conversions.h>
-namespace armarx::navigation::platform_controller::platform_trajectory
+namespace armarx::navigation::platform_controller::platform_global_trajectory
{
const NJointControllerRegistration<Controller> Registration(
- common::ControllerTypeNames.to_name(common::ControllerType::PlatformTrajectory));
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformGlobalTrajectory));
Controller::Controller(const RobotUnitPtr& robotUnit,
const NJointControllerConfigPtr& config,
@@ -25,7 +23,7 @@ namespace armarx::navigation::platform_controller::platform_trajectory
{
ARMARX_IMPORTANT << "Creating "
<< common::ControllerTypeNames.to_name(
- common::ControllerType::PlatformTrajectory);
+ common::ControllerType::PlatformGlobalTrajectory);
// config
ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
ARMARX_CHECK_EXPRESSION(cfg);
@@ -57,7 +55,7 @@ namespace armarx::navigation::platform_controller::platform_trajectory
Controller::getClassName(const Ice::Current& iceCurrent) const
{
return armarx::navigation::common::ControllerTypeNames.to_name(
- armarx::navigation::common::ControllerType::PlatformTrajectory);
+ armarx::navigation::common::ControllerType::PlatformGlobalTrajectory);
}
void
@@ -105,7 +103,7 @@ namespace armarx::navigation::platform_controller::platform_trajectory
}
// update controller
- const armarx::navigation::traj_ctrl::TrajectoryControllerResult result =
+ const armarx::navigation::traj_ctrl::global::TrajectoryControllerResult result =
trajectoryFollowingController->control(
configBuffer.getUpToDateReadBuffer().targets.trajectory);
@@ -118,8 +116,8 @@ namespace armarx::navigation::platform_controller::platform_trajectory
}
void
- Controller::onPublish(const SensorAndControl& sac,
- const DebugDrawerInterfacePrx& debugDrawer,
+ Controller::onPublish(const SensorAndControl& /*sac*/,
+ const DebugDrawerInterfacePrx& /*debugDrawer*/,
const DebugObserverInterfacePrx& debugObservers)
{
StringVariantBaseMap datafields;
@@ -127,10 +125,11 @@ namespace armarx::navigation::platform_controller::platform_trajectory
datafields["vx"] = new Variant(rtGetControlStruct().platformVelocityTargets.x);
datafields["vy"] = new Variant(rtGetControlStruct().platformVelocityTargets.y);
datafields["vyaw"] = new Variant(rtGetControlStruct().platformVelocityTargets.yaw);
- datafields["trajectory_points"] = new Variant(configBuffer.getUpToDateReadBuffer().targets.trajectory.points().size());
+ datafields["trajectory_points"] =
+ new Variant(configBuffer.getUpToDateReadBuffer().targets.trajectory.points().size());
debugObservers->setDebugChannel(
- common::ControllerTypeNames.to_name(common::ControllerType::PlatformTrajectory),
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformGlobalTrajectory),
datafields);
}
@@ -166,4 +165,4 @@ namespace armarx::navigation::platform_controller::platform_trajectory
Controller::~Controller() = default;
-} // namespace armarx::navigation::platform_controller::platform_trajectory
+} // namespace armarx::navigation::platform_controller::platform_global_trajectory
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h
similarity index 76%
rename from source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
rename to source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h
index 2b47dc9808b4a127f86919ae733ebd259acc25cb..ad6c2701cdde73db665780e7030f43c4065a1578 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
+++ b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h
@@ -29,9 +29,10 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h>
#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
#include <armarx/navigation/core/types.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
namespace armarx
{
@@ -40,16 +41,16 @@ namespace armarx
} // namespace armarx
-namespace armarx::navigation::platform_controller::platform_trajectory
+namespace armarx::navigation::platform_controller::platform_global_trajectory
{
struct Targets
{
- core::Trajectory trajectory;
+ core::GlobalTrajectory trajectory;
};
struct Config
{
- using Params = traj_ctrl::TrajectoryFollowingControllerParams;
+ using Params = traj_ctrl::global::TrajectoryFollowingControllerParams;
Params params;
Targets targets;
@@ -112,15 +113,6 @@ namespace armarx::navigation::platform_controller::platform_trajectory
void rtPreActivateController() override;
private:
- // [[nodiscard]] bool initializeQPIK();
-
- // void updateDebugStatus();
-
- // struct DebugStatus
- // {
- // std::map<std::string, float> data;
- // };
-
TripleBuffer<Config> configBuffer;
// internal
@@ -129,21 +121,10 @@ namespace armarx::navigation::platform_controller::platform_trajectory
ControlTargetHolonomicPlatformVelocity* platformTarget;
- std::optional<traj_ctrl::TrajectoryFollowingController> trajectoryFollowingController;
+ std::optional<traj_ctrl::global::TrajectoryFollowingController> trajectoryFollowingController;
Devices getDevices(const VirtualRobot::RobotNodeSet& rns);
-
- // NameValueMap getJointVelocities(const NameValueMap& initialJointValues,
- // const NameValueMap& targetJointValues,
- // const std::set<std::string>& joints);
-
- // Eigen::VectorXd toOptikRobotState(const NameValueMap& jointValues) const;
-
- // NameValueMap getJointValuesWithVirtualPlatformJoints();
-
- // NameValueMap getPlatformJointVelocity(const NameValueMap& initialJointAngles,
- // const NameValueMap& targetJointAngles) const;
};
-} // namespace armarx::navigation::platform_controller::platform_trajectory
+} // namespace armarx::navigation::platform_controller::platform_global_trajectory
diff --git a/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.cpp b/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e1ce20cb59b217d19b20b05dada7ba9d22080690
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.cpp
@@ -0,0 +1,167 @@
+#include "PlatformLocalTrajectoryController.h"
+
+#include "ArmarXCore/core/ArmarXObjectScheduler.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/CycleUtil.h"
+
+#include "RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h"
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
+
+#include <armarx/control/common/type.h>
+#include <armarx/navigation/common/controller_types.h>
+#include <armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/aron_conversions.h>
+
+namespace armarx::navigation::platform_controller::platform_local_trajectory
+{
+ const NJointControllerRegistration<Controller> Registration(
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformLocalTrajectory));
+
+ Controller::Controller(const RobotUnitPtr& robotUnit,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&)
+ {
+ ARMARX_IMPORTANT << "Creating "
+ << common::ControllerTypeNames.to_name(
+ common::ControllerType::PlatformLocalTrajectory);
+ // config
+ ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION(cfg);
+ ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
+
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+
+ const auto robot = useSynchronizedRtRobot();
+
+ platformTarget = useControlTarget(robotUnit->getRobotPlatformName(),
+ ControlModes::HolonomicPlatformVelocity)
+ ->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION(platformTarget)
+ << "The actuator " << robotUnit->getRobotPlatformName() << " has no control mode "
+ << ControlModes::HolonomicPlatformVelocity;
+
+ const auto configData = ::armarx::fromAron<arondto::Config, Config>(cfg->config);
+ const auto trajectoryFollowingControllerParams = configData.params;
+
+ reinitTripleBuffer({});
+ configBuffer.reinitAllBuffers(configData);
+
+ trajectoryFollowingController.emplace(robot, trajectoryFollowingControllerParams);
+
+ ARMARX_INFO << "Init done.";
+ }
+
+ std::string
+ Controller::getClassName(const Ice::Current& iceCurrent) const
+ {
+ return armarx::navigation::common::ControllerTypeNames.to_name(
+ armarx::navigation::common::ControllerType::PlatformLocalTrajectory);
+ }
+
+ void
+ Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtUpdateControlStruct();
+
+ platformTarget->velocityX = rtGetControlStruct().platformVelocityTargets.x;
+ platformTarget->velocityY = rtGetControlStruct().platformVelocityTargets.y;
+ platformTarget->velocityRotation = rtGetControlStruct().platformVelocityTargets.yaw;
+ }
+
+ void
+ Controller::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
+ const Ice::Current& iceCurrent)
+ {
+ ARMARX_IMPORTANT << "Controller::updateConfig";
+
+ // TODO maybe update pid controller
+
+ auto updateConfig = ::armarx::fromAron<arondto::Config, Config>(dto);
+ configBuffer.reinitAllBuffers(updateConfig);
+
+ ARMARX_INFO << "Trajectory with " << updateConfig.targets.trajectory.points().size();
+
+ ARMARX_IMPORTANT << "done Controller::updateConfig";
+ }
+
+ void
+ Controller::additionalTask()
+ {
+ ARMARX_CHECK(trajectoryFollowingController.has_value());
+
+ // if trajectory is empty, set velocity to 0
+ if (configBuffer.getUpToDateReadBuffer().targets.trajectory.points().empty())
+ {
+ ARMARX_INFO << deactivateSpam(1) << "Trajectory is empty!";
+
+ getWriterControlStruct().platformVelocityTargets.x = 0;
+ getWriterControlStruct().platformVelocityTargets.y = 0;
+ getWriterControlStruct().platformVelocityTargets.yaw = 0;
+ writeControlStruct();
+ return;
+ }
+
+ // update controller
+ const auto result =
+ trajectoryFollowingController->control(configBuffer.getUpToDateReadBuffer().targets.trajectory);
+
+ // store result
+ getWriterControlStruct().platformVelocityTargets.x = result.twist.linear.x();
+ getWriterControlStruct().platformVelocityTargets.y = result.twist.linear.y();
+ getWriterControlStruct().platformVelocityTargets.yaw = result.twist.angular.z();
+
+ writeControlStruct();
+ }
+
+ void
+ Controller::onPublish(const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& debugDrawer,
+ const DebugObserverInterfacePrx& debugObservers)
+ {
+ StringVariantBaseMap datafields;
+
+ datafields["vx"] = new Variant(rtGetControlStruct().platformVelocityTargets.x);
+ datafields["vy"] = new Variant(rtGetControlStruct().platformVelocityTargets.y);
+ datafields["vyaw"] = new Variant(rtGetControlStruct().platformVelocityTargets.yaw);
+ datafields["trajectory_points"] =
+ new Variant(configBuffer.getUpToDateReadBuffer().targets.trajectory.points().size());
+
+ debugObservers->setDebugChannel(
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformLocalTrajectory),
+ datafields);
+ }
+
+ void
+ Controller::onInitNJointController()
+ {
+ runTask("PlatformTrajectoryControllerAdditionalTask",
+ [&]
+ {
+ CycleUtil c(10);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ ARMARX_IMPORTANT << "Create a new thread alone PlatformTrajectory controller";
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive() and rtReady.load())
+ {
+ ARMARX_VERBOSE << "additional task";
+ additionalTask();
+ }
+ c.waitForCycleDuration();
+ }
+ });
+
+ ARMARX_INFO << "PlatformTrajectoryVelocityController::onInitNJointController";
+ }
+
+ void
+ Controller::rtPreActivateController()
+ {
+ rtReady.store(true);
+ }
+
+
+ Controller::~Controller() = default;
+
+} // namespace armarx::navigation::platform_controller::platform_local_trajectory
diff --git a/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h b/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h
new file mode 100644
index 0000000000000000000000000000000000000000..8aa75fb7ef5767e743572b431dfd863d8e3bc7ad
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h
@@ -0,0 +1,129 @@
+/**
+ * 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 <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
+
+#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
+
+namespace armarx
+{
+ class ControlTargetHolonomicPlatformVelocity;
+ class SensorValueHolonomicPlatformWithAbsolutePosition;
+} // namespace armarx
+
+
+namespace armarx::navigation::platform_controller::platform_local_trajectory
+{
+ struct Targets
+ {
+ core::LocalTrajectory trajectory;
+ };
+
+ struct Config
+ {
+ using Params = traj_ctrl::local::TrajectoryFollowingControllerParams;
+
+ Params params;
+ Targets targets;
+ };
+
+ struct Target
+ {
+ struct
+ {
+ double x = 0;
+ double y = 0;
+ double yaw = 0;
+ } platformVelocityTargets;
+
+ void
+ reset()
+ {
+ platformVelocityTargets.x = 0;
+ platformVelocityTargets.y = 0;
+ platformVelocityTargets.yaw = 0;
+ }
+ };
+
+ struct Devices
+ {
+ ControlTargetHolonomicPlatformVelocity* platformTarget;
+ };
+
+ using NameValueMap = std::map<std::string, float>;
+
+ class Controller :
+ virtual public NJointControllerWithTripleBuffer<Target>,
+ virtual public armarx::control::ConfigurableNJointControllerInterface
+ {
+ public:
+ using ConfigPtrT = control::ConfigurableNJointControllerConfigPtr;
+
+ Controller(const RobotUnitPtr& robotUnit,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
+
+ ~Controller() override;
+
+ std::string getClassName(const Ice::Current& iceCurrent = Ice::emptyCurrent) const override;
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+
+ void updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
+ const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
+
+ protected:
+ void additionalTask();
+ void onPublish(const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& debugDrawer,
+ const DebugObserverInterfacePrx& debugObservers) override;
+
+
+ void onInitNJointController() override;
+ void rtPreActivateController() override;
+
+ private:
+ TripleBuffer<Config> configBuffer;
+
+ // internal
+ std::atomic_bool rtFirstRun = true;
+ std::atomic_bool rtReady = false;
+
+ ControlTargetHolonomicPlatformVelocity* platformTarget;
+
+ std::optional<traj_ctrl::local::TrajectoryFollowingController> trajectoryFollowingController;
+
+
+ Devices getDevices(const VirtualRobot::RobotNodeSet& rns);
+ };
+
+} // namespace armarx::navigation::platform_controller::platform_local_trajectory
diff --git a/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml b/source/armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.xml
similarity index 56%
rename from source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
rename to source/armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.xml
index 44c81bf444f9fc00846dbf18c5b426a4a15b469b..d992e66d0908919ef77240b6d30290444d74bc5e 100644
--- a/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
+++ b/source/armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.xml
@@ -4,38 +4,31 @@
<Include include="armarx/navigation/core/aron/TwistLimits.xml" />
<Include include="armarx/navigation/core/aron/PIDParams.xml" />
<Include include="armarx/navigation/core/aron/Trajectory.xml" />
- <Include include="armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.xml" />
+ <Include include="armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.xml" />
<!-- <Include include="<armarx/control/common/control_law/aron/TaskspaceImpedanceControllerConfig.xml>" /> -->
</AronIncludes>
- <CodeIncludes>
- <Include include="<Eigen/Core>" />
- <Include include="<armarx/navigation/core/aron/TwistLimits.aron.generated.h>" />
- <Include include="<armarx/navigation/core/aron/PIDParams.aron.generated.h>" />
- <Include include="<armarx/navigation/core/aron/Trajectory.aron.generated.h>" />
- <Include include="<armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h>" />
- </CodeIncludes>
<GenerateTypes>
- <Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Params'>
+ <Object name='armarx::navigation::platform_controller::platform_global_trajectory::arondto::Params'>
<ObjectChild key='twistLimits'>
<armarx::navigation::core::arondto::TwistLimits />
</ObjectChild>
</Object>
- <Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Targets'>
+ <Object name='armarx::navigation::platform_controller::platform_global_trajectory::arondto::Targets'>
<ObjectChild key='trajectory'>
- <armarx::navigation::core::arondto::Trajectory />
+ <armarx::navigation::core::arondto::GlobalTrajectory />
</ObjectChild>
</Object>
- <Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Config'>
+ <Object name='armarx::navigation::platform_controller::platform_global_trajectory::arondto::Config'>
<ObjectChild key='params'>
- <armarx::navigation::traj_ctrl::arondto::TrajectoryFollowingControllerParams />
+ <armarx::navigation::traj_ctrl::global::arondto::TrajectoryFollowingControllerParams />
</ObjectChild>
<ObjectChild key='targets'>
- <armarx::navigation::platform_controller::platform_trajectory::arondto::Targets />
+ <armarx::navigation::platform_controller::platform_global_trajectory::arondto::Targets />
</ObjectChild>
</Object>
diff --git a/source/armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.xml b/source/armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1223bb15b11960203e84a44b00456a76f9333810
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ <Include include="armarx/navigation/core/aron/TwistLimits.xml" />
+ <Include include="armarx/navigation/core/aron/PIDParams.xml" />
+ <Include include="armarx/navigation/core/aron/Trajectory.xml" />
+ <Include include="armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.xml" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name='armarx::navigation::platform_controller::platform_local_trajectory::arondto::Params'>
+ <ObjectChild key='twistLimits'>
+ <armarx::navigation::core::arondto::TwistLimits />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::platform_controller::platform_local_trajectory::arondto::Targets'>
+ <ObjectChild key='trajectory'>
+ <armarx::navigation::core::arondto::LocalTrajectory />
+ </ObjectChild>
+ </Object>
+
+
+ <Object name='armarx::navigation::platform_controller::platform_local_trajectory::arondto::Config'>
+ <ObjectChild key='params'>
+ <armarx::navigation::traj_ctrl::local::arondto::TrajectoryFollowingControllerParams />
+ </ObjectChild>
+
+ <ObjectChild key='targets'>
+ <armarx::navigation::platform_controller::platform_local_trajectory::arondto::Targets />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/platform_controller/aron_conversions.cpp b/source/armarx/navigation/platform_controller/aron_conversions.cpp
index 0f32b69e9ea4195029c3fec019d75951a7a3ae78..87d6dc9df80098e4ed020261d7ee38b40d33e710 100644
--- a/source/armarx/navigation/platform_controller/aron_conversions.cpp
+++ b/source/armarx/navigation/platform_controller/aron_conversions.cpp
@@ -23,28 +23,52 @@
#include <RobotAPI/libraries/aron/common/aron_conversions/eigen.h>
-#include <armarx/control/common/aron_conversions.h>
-#include <armarx/navigation/platform_controller/PlatformTrajectoryController.h>
#include <armarx/navigation/core/aron_conversions.h>
-#include <armarx/navigation/trajectory_control/aron_conversions.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h>
+#include <armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h>
+#include <armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/trajectory_control/local/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/global/aron_conversions.h>
-
-namespace armarx::navigation::platform_controller::platform_trajectory
+namespace armarx::navigation::platform_controller
{
- void
- fromAron(const arondto::Targets& dto, Targets& bo)
+ namespace platform_global_trajectory
{
- fromAron(dto.trajectory, bo.trajectory);
- }
- void
- fromAron(const arondto::Config& dto, Config& bo)
+ void
+ fromAron(const arondto::Targets& dto, Targets& bo)
+ {
+ fromAron(dto.trajectory, bo.trajectory);
+ }
+
+ void
+ fromAron(const arondto::Config& dto, Config& bo)
+ {
+ fromAron(dto.params, bo.params);
+ fromAron(dto.targets, bo.targets);
+ }
+
+ } // namespace platform_global_trajectory
+
+ namespace platform_local_trajectory
{
- fromAron(dto.params, bo.params);
- fromAron(dto.targets, bo.targets);
- }
-} // namespace armarx::navigation::platform_controller::platform_trajectory
+ void
+ fromAron(const arondto::Targets& dto, Targets& bo)
+ {
+ fromAron(dto.trajectory, bo.trajectory);
+ }
+
+ void
+ fromAron(const arondto::Config& dto, Config& bo)
+ {
+ fromAron(dto.params, bo.params);
+ fromAron(dto.targets, bo.targets);
+ }
+
+ } // namespace platform_local_trajectory
+
+} // namespace armarx::navigation::platform_controller
diff --git a/source/armarx/navigation/platform_controller/aron_conversions.h b/source/armarx/navigation/platform_controller/aron_conversions.h
index bd5518b1b0f2e11f0cb3b2e0885f514073b5e92b..35682c9dcc8658e4e2e366764ddce528e4386d38 100644
--- a/source/armarx/navigation/platform_controller/aron_conversions.h
+++ b/source/armarx/navigation/platform_controller/aron_conversions.h
@@ -21,15 +21,55 @@
#pragma once
+#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h>
-namespace armarx::navigation::platform_controller::platform_trajectory
+
+namespace armarx
{
- namespace arondto
+ template <typename AronType, typename T>
+ T
+ fromAron(const ::armarx::aron::data::dto::DictPtr& dto)
{
- class Config;
+ static_assert(
+ std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass, AronType>::value,
+ "AronType must be an ARON generated type");
+
+ const auto dtoConfig = AronType::FromAron(dto);
+
+ T config;
+ fromAron(dtoConfig, config);
+
+ return config;
}
+} // namespace armarx
+
+
+namespace armarx::navigation::platform_controller
+{
+ namespace platform_global_trajectory
+ {
+ namespace arondto
+ {
+ class Config;
+ }
+
+ struct Config;
+
+ void fromAron(const arondto::Config& dto, Config& bo);
+
+ } // namespace platform_global_trajectory
+
+ namespace platform_local_trajectory
+ {
+ namespace arondto
+ {
+ class Config;
+ }
+
+ struct Config;
+
+ void fromAron(const arondto::Config& dto, Config& bo);
- struct Config;
+ } // namespace platform_local_trajectory
- void fromAron(const arondto::Config& dto, Config& bo);
-} // namespace armarx::navigation::platform_controller::platform_trajectory
+} // namespace armarx::navigation::platform_controller
diff --git a/source/armarx/navigation/platform_controller/controller_descriptions.h b/source/armarx/navigation/platform_controller/controller_descriptions.h
index d352bed138bb228ce71ca0fd0ec3d1a28a936ea7..30ce76d9bd1b630bff6dfba58c25848aecc61f6b 100644
--- a/source/armarx/navigation/platform_controller/controller_descriptions.h
+++ b/source/armarx/navigation/platform_controller/controller_descriptions.h
@@ -23,22 +23,37 @@
#include <armarx/control/client/ControllerDescription.h>
#include <armarx/navigation/common/controller_types.h>
-#include <armarx/navigation/platform_controller/PlatformTrajectoryController.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h>
+#include <armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h>
+#include <armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.aron.generated.h>
namespace armarx::control::client
{
template <>
- struct ControllerDescription<armarx::navigation::common::ControllerType::PlatformTrajectory>
+ struct ControllerDescription<armarx::navigation::common::ControllerType::PlatformGlobalTrajectory>
{
using AronDTO =
- armarx::navigation::platform_controller::platform_trajectory::arondto::Config;
+ armarx::navigation::platform_controller::platform_global_trajectory::arondto::Config;
- using BO = armarx::navigation::platform_controller::platform_trajectory::Config;
+ using BO = armarx::navigation::platform_controller::platform_global_trajectory::Config;
// constexpr static const char* name = armarx::navigation::common::ControllerTypeNames.to_name(armarx::navigation::common::ControllerType::PlatformTrajectory);
constexpr static const char* name =
- armarx::navigation::common::PlatformTrajectoryControllerName;
+ armarx::navigation::common::PlatformGlobalTrajectoryControllerName;
+ };
+
+ template <>
+ struct ControllerDescription<armarx::navigation::common::ControllerType::PlatformLocalTrajectory>
+ {
+ using AronDTO =
+ armarx::navigation::platform_controller::platform_local_trajectory::arondto::Config;
+
+ using BO = armarx::navigation::platform_controller::platform_local_trajectory::Config;
+
+ // constexpr static const char* name = armarx::navigation::common::ControllerTypeNames.to_name(armarx::navigation::common::ControllerType::PlatformTrajectory);
+ constexpr static const char* name =
+ armarx::navigation::common::PlatformLocalTrajectoryControllerName;
};
} // namespace armarx::control::client
diff --git a/source/armarx/navigation/rooms/RoomNavigationTargetCreator.cpp b/source/armarx/navigation/rooms/RoomNavigationTargetCreator.cpp
index 288fb17230c7bdbe17353b4760a07c417af9d329..666aa7ec690de8e764fc0e8f3a0f8dcbf0530808 100644
--- a/source/armarx/navigation/rooms/RoomNavigationTargetCreator.cpp
+++ b/source/armarx/navigation/rooms/RoomNavigationTargetCreator.cpp
@@ -1,10 +1,16 @@
#include "RoomNavigationTargetCreator.h"
+#include <SimoxUtility/color/Color.h>
+
#include "ArmarXCore/core/logging/Logging.h"
-#include "armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h"
-#include "armarx/navigation/conversions/eigen.h"
-#include "armarx/navigation/core/basic_types.h"
+#include "RobotAPI/components/ArViz/Client/Client.h"
+#include "RobotAPI/components/ArViz/Client/Elements.h"
+
+#include "armarx/navigation/algorithms/visualization.h"
+#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
+#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/core/basic_types.h>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/reverse.hpp>
@@ -18,7 +24,8 @@ namespace armarx::navigation::rooms
RoomNavigationTargetCreator::getClosestPositionOutsideOfRoom(
const algorithms::Costmap& obstacleDistancesCostmap,
const Room& room,
- const core::Position& global_P_robot) const
+ const core::Position& global_P_robot,
+ viz::Client* arviz) const
{
using ShortestPathFasterAlgorithm = algorithms::spfa::ShortestPathFasterAlgorithm;
@@ -39,6 +46,9 @@ namespace armarx::navigation::rooms
const std::size_t cX = roomCostmap.getGrid().rows();
const std::size_t cY = roomCostmap.getGrid().cols();
+ // invalidate everything that is in collision
+ // roomCostmap.getMutableMask().value() = roomCostmap.getGrid().array() >= 0.5;
+
#pragma omp parallel for schedule(static)
for (unsigned int x = 0; x < cX; x++)
{
@@ -51,16 +61,37 @@ namespace armarx::navigation::rooms
pos3d.z() = room.zFloor + room.height / 2; // just at the center (height)
// invalid outside of the room
- roomCostmap.getMutableMask().value()(x, y) = room.isInside(pos3d);
+ const bool isInside = room.isInside(pos3d);
+
+ roomCostmap.getMutableMask().value()(x, y) &= isInside;
+ roomCostmap.getMutableGrid()(x, y) *= static_cast<float>(isInside);
}
}
+ ARMARX_INFO << "Closest position to room: " << roomCostmap.optimum().position;
+
const core::Position closestReachablePositionOnRoomBoundary =
conv::to3D(roomCostmap.optimum().position);
+
// We don't stop here. We want to place the robot in front of the room at a
- const auto plan =
- spfa.plan(conv::to2D(global_P_robot), conv::to2D(closestReachablePositionOnRoomBoundary));
+ const auto plan = spfa.plan(conv::to2D(global_P_robot),
+ conv::to2D(closestReachablePositionOnRoomBoundary));
+
+ if (arviz != nullptr)
+ {
+ auto layer = arviz->layer("RoomNavigationTargetCreator");
+ algorithms::visualize(roomCostmap, layer, "room_costmap");
+
+ layer.add(viz::Sphere("closestReachablePositionOnRoomBoundary")
+ .position(closestReachablePositionOnRoomBoundary)
+ .radius(50)
+ .color(simox::Color::blue()));
+
+ arviz->commit(layer);
+ }
+
+
ARMARX_CHECK(plan.success);
const auto posInFrontOfRoom = [&]() -> core::Position
@@ -70,9 +101,11 @@ namespace armarx::navigation::rooms
const auto reversedPlan = rv::reverse(plan.path) | r::to_vector;
+ const auto goalPos = conv::to2D(closestReachablePositionOnRoomBoundary);
+
for (const auto& pos : reversedPlan)
{
- if ((pos - reversedPlan.front()).norm() < params.distanceToRoomEntry)
+ if ((pos - goalPos).norm() > params.distanceToRoomEntry)
{
return conv::to3D(pos);
}
@@ -82,6 +115,18 @@ namespace armarx::navigation::rooms
return conv::to3D(reversedPlan.back()); // the current robot pos
}();
+ if (arviz != nullptr)
+ {
+ auto layer = arviz->layer("RoomNavigationTargetCreator_target");
+
+ layer.add(viz::Sphere("posInFrontOfRoom")
+ .position(posInFrontOfRoom)
+ .radius(50)
+ .color(simox::Color::green()));
+
+ arviz->commit(layer);
+ }
+
return {.global_P_roomEntry = closestReachablePositionOnRoomBoundary,
.global_P_robot = posInFrontOfRoom};
}
diff --git a/source/armarx/navigation/rooms/RoomNavigationTargetCreator.h b/source/armarx/navigation/rooms/RoomNavigationTargetCreator.h
index c83ca79466fc9f2caecad55722b381a1a09551d4..5e72cece278efb36e8191179f5cf61671c632916 100644
--- a/source/armarx/navigation/rooms/RoomNavigationTargetCreator.h
+++ b/source/armarx/navigation/rooms/RoomNavigationTargetCreator.h
@@ -21,9 +21,15 @@
#pragma once
-#include "armarx/navigation/algorithms/Costmap.h"
-#include "armarx/navigation/core/basic_types.h"
-#include "armarx/navigation/rooms/types.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/rooms/types.h>
+
+namespace armarx::viz
+{
+ class Client;
+}
+
namespace armarx::navigation::rooms
{
@@ -32,18 +38,20 @@ namespace armarx::navigation::rooms
public:
struct Params
{
- float distanceToRoomEntry = 1000;
+ float distanceToRoomEntry = 1000;
};
RoomNavigationTargetCreator(const Params& params);
- struct Result{
+ struct Result
+ {
core::Position global_P_roomEntry;
core::Position global_P_robot; // where the robot should be placed
};
Result getClosestPositionOutsideOfRoom(const algorithms::Costmap& obstacleDistancesCostmap,
- const Room& room,
- const core::Position& global_P_robot) const;
+ const Room& room,
+ const core::Position& global_P_robot,
+ viz::Client* arviz = nullptr) const;
protected:
private:
diff --git a/source/armarx/navigation/rooms/aron_conversions.cpp b/source/armarx/navigation/rooms/aron_conversions.cpp
index c8b2e9204386f357336298ceb75aef633883f1b9..280afa4ce54a7fcad2cb08f8d0fb3a1c71c4afa7 100644
--- a/source/armarx/navigation/rooms/aron_conversions.cpp
+++ b/source/armarx/navigation/rooms/aron_conversions.cpp
@@ -1,5 +1,5 @@
#include "aron_conversions.h"
-#include "armarx/navigation/conversions/eigen.h"
+#include <armarx/navigation/conversions/eigen.h>
namespace armarx::navigation::rooms
diff --git a/source/armarx/navigation/rooms/types.cpp b/source/armarx/navigation/rooms/types.cpp
index f5d1a2e7ee7e2ad63cc78ba8bea73bc06cb60a90..d244e6504dd0f55cfe01e203dbfa1f2f0d2c5fd0 100644
--- a/source/armarx/navigation/rooms/types.cpp
+++ b/source/armarx/navigation/rooms/types.cpp
@@ -4,8 +4,8 @@
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
-#include "armarx/navigation/conversions/eigen.h"
-#include "armarx/navigation/util/geometry.h"
+#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/util/geometry.h>
namespace armarx::navigation::rooms
{
diff --git a/source/armarx/navigation/server/NavigationStack.h b/source/armarx/navigation/server/NavigationStack.h
index c2a76f9a2269400f62d3e699d503036159db7a47..c5d80fc2c5841285bb8f590979ad41efd516e4da 100644
--- a/source/armarx/navigation/server/NavigationStack.h
+++ b/source/armarx/navigation/server/NavigationStack.h
@@ -25,7 +25,8 @@
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/safety_control/SafetyController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::server
{
@@ -33,7 +34,7 @@ namespace armarx::navigation::server
struct NavigationStack
{
global_planning::GlobalPlannerPtr globalPlanner;
- loc_plan::LocalPlannerPtr localPlanner = nullptr;
+ local_planning::LocalPlannerPtr localPlanner = nullptr;
// traj_ctrl::TrajectoryControllerPtr trajectoryControl;
// safe_ctrl::SafetyControllerPtr safetyControl = nullptr;
};
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index cc30160254ad3470a7e2ff571bbe7460aa20d459..17eb7d47d42b88f6ffa1a40e8e9c4f55ad5bdf12 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -2,18 +2,19 @@
#include <algorithm>
#include <chrono>
+#include <cmath>
+#include <cstddef>
#include <optional>
#include <thread>
-#include <range/v3/view.hpp>
-#include <range/v3/view/reverse.hpp>
-
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/subgraph.hpp>
#include <Eigen/Geometry>
+#include <Eigen/src/Geometry/AngleAxis.h>
+#include <Eigen/src/Geometry/Translation.h>
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <VirtualRobot/Robot.h>
@@ -25,7 +26,7 @@
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
-#include "armarx/navigation/local_planning/LocalPlanner.h"
+#include "armarx/navigation/memory/client/graph/Reader.h"
#include "range/v3/algorithm/reverse.hpp"
#include "range/v3/range/conversion.hpp"
#include <SemanticObjectRelations/Shapes/Shape.h>
@@ -38,10 +39,13 @@
#include <armarx/navigation/core/events.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
+#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/server/GraphBuilder.h>
#include <armarx/navigation/server/StackResult.h>
#include <armarx/navigation/server/monitoring/GoalReachedMonitor.h>
#include <armarx/navigation/server/scene_provider/SceneProviderInterface.h>
+#include <range/v3/view.hpp>
+#include <range/v3/view/reverse.hpp>
namespace armarx::navigation::server
@@ -59,7 +63,7 @@ namespace armarx::navigation::server
config{config}, srv{services}
{
ARMARX_CHECK_NOT_NULL(srv.sceneProvider) << "The scene provider must be set!";
- ARMARX_CHECK_NOT_NULL(services.executor) << "The executor service must be set!";
+ // ARMARX_CHECK_NOT_NULL(services.executor) << "The executor service must be set!";
ARMARX_CHECK_NOT_NULL(services.publisher) << "The publisher service must be set!";
}
@@ -157,17 +161,42 @@ namespace armarx::navigation::server
const core::Pose global_T_robot(srv.sceneProvider->scene().robot->getGlobalPose());
ARMARX_VERBOSE << "Initial robot pose: " << global_T_robot.matrix();
+ std::transform(std::begin(waypoints),
+ std::end(waypoints),
+ std::back_inserter(globalWaypoints),
+ [&](const core::Pose& p) { return global_T_robot * p; });
+ break;
+ }
+
+ ARMARX_TRACE;
+ moveToAbsolute(globalWaypoints);
+ }
+
+ void
+ Navigator::update(const std::vector<core::Pose>& waypoints,
+ core::NavigationFrame navigationFrame)
+ {
+ ARMARX_INFO << "Received update() request.";
+
+ std::vector<core::Pose> globalWaypoints;
+ switch (navigationFrame)
+ {
+ case core::NavigationFrame::Absolute:
+ globalWaypoints = waypoints;
+ break;
+ case core::NavigationFrame::Relative:
+ globalWaypoints.reserve(waypoints.size());
std::transform(
std::begin(waypoints),
std::end(waypoints),
std::back_inserter(globalWaypoints),
[&](const core::Pose& p)
- { return global_T_robot * p; });
+ { return core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()) * p; });
break;
}
ARMARX_TRACE;
- moveToAbsolute(globalWaypoints);
+ updateAbsolute(globalWaypoints);
}
using GraphPath = std::vector<semrel::ShapeID>;
@@ -290,7 +319,7 @@ namespace armarx::navigation::server
return shortestPath;
}
- core::Trajectory
+ core::GlobalTrajectory
convertToTrajectory(const GraphPath& shortestPath, const core::Graph& graph)
{
ARMARX_TRACE;
@@ -300,7 +329,7 @@ namespace armarx::navigation::server
// ARMARX_CHECK_EQUAL(shortestPath.size() - 1, shortestPath.edges.size());
- std::vector<core::TrajectoryPoint> trajectoryPoints;
+ std::vector<core::GlobalTrajectoryPoint> trajectoryPoints;
// TODO add the start
// trajectoryPoints.push_back(core::TrajectoryPoint{
@@ -343,7 +372,7 @@ namespace armarx::navigation::server
// we have a trajectory
// FIXME trajectory points can be invalid => more points than expected. Why?
ARMARX_TRACE;
- const std::vector<core::TrajectoryPoint> edgeTrajectoryPoints =
+ const std::vector<core::GlobalTrajectoryPoint> edgeTrajectoryPoints =
edge.attrib().trajectory->points();
// if trajectory is being initialized, include the start, otherwise skip it
@@ -382,11 +411,11 @@ namespace armarx::navigation::server
// FIXME variable
const float point2pointVelocity = 400;
- const core::TrajectoryPoint currentTrajPt = {
+ const core::GlobalTrajectoryPoint currentTrajPt = {
.waypoint = {.pose = graph.vertex(shortestPath.at(i)).attrib().getPose()},
.velocity = point2pointVelocity};
- const core::TrajectoryPoint nextTrajPt{
+ const core::GlobalTrajectoryPoint nextTrajPt{
.waypoint = {.pose =
graph.vertex(shortestPath.at(i + 1)).attrib().getPose()},
.velocity = 0};
@@ -597,7 +626,7 @@ namespace armarx::navigation::server
// convert graph / vertex chain to trajectory
ARMARX_TRACE;
- core::Trajectory globalPlanTrajectory = convertToTrajectory(shortestPath, graph);
+ core::GlobalTrajectory globalPlanTrajectory = convertToTrajectory(shortestPath, graph);
// globalPlanTrajectory.setMaxVelocity(1000); // FIXME
@@ -611,7 +640,7 @@ namespace armarx::navigation::server
// the following is similar to moveToAbsolute
// TODO(fabian.reister): remove code duplication
- srv.executor->execute(globalPlan->trajectory);
+ // srv.executor->execute(globalPlan->trajectory);
ARMARX_TRACE;
srv.publisher->globalTrajectoryUpdated(globalPlan.value());
@@ -631,14 +660,23 @@ namespace armarx::navigation::server
ARMARX_TRACE;
- ARMARX_INFO << "Planning local trajectory enabled";
+ ARMARX_INFO << "Starting stack.";
+
+ // FIXME instead of PeriodicTask, use RunningTask.
runningTask = new PeriodicTask<Navigator>(this,
&Navigator::run,
config.general.tasks.replanningUpdatePeriod,
false,
- "LocalPlannerTask");
+ "PeriodicTask");
runningTask->start();
+ // FIXME create separate function for this.
+ if(hasLocalPlanner())
+ {
+ srv.executor->start(ExecutorInterface::ControllerType::LocalTrajectory);
+ } else {
+ srv.executor->start(ExecutorInterface::ControllerType::GlobalTrajectory);
+ }
// Could be required if pauseMovement() has been called in the past.
resume();
@@ -650,13 +688,14 @@ namespace armarx::navigation::server
Navigator::moveToAbsolute(const std::vector<core::Pose>& waypoints)
{
ARMARX_TRACE;
+
// if this navigator is in use, stop the movement ...
pause();
// ... and all threads
stopAllThreads();
// FIXME remove
- std::this_thread::sleep_for(std::chrono::seconds(1));
+ //std::this_thread::sleep_for(std::chrono::seconds(1));
ARMARX_TRACE;
ARMARX_CHECK_NOT_EMPTY(waypoints);
@@ -706,7 +745,53 @@ namespace armarx::navigation::server
ARMARX_TRACE;
srv.publisher->globalTrajectoryUpdated(globalPlan.value());
srv.introspector->onGlobalPlannerResult(globalPlan.value());
- srv.executor->execute(globalPlan->trajectory);
+
+ if(not hasLocalPlanner())
+ {
+ updateExecutor(globalPlan.value());
+ }
+
+ ARMARX_INFO << "Global planning completed. Will now start all required threads";
+ ARMARX_TRACE;
+
+ startStack();
+
+ ARMARX_INFO << "Movement started.";
+ }
+
+
+ void
+ Navigator::updateAbsolute(const std::vector<core::Pose>& waypoints)
+ {
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_EMPTY(waypoints);
+
+
+ // global planner
+ ARMARX_INFO << "Planning global trajectory";
+ ARMARX_CHECK_NOT_NULL(config.stack.globalPlanner);
+ // TODO plan on multiple waypoints, ignoring waypoints for now
+ // idea: compute multiple global trajectories, one for each segment between waypoints.
+
+ srv.introspector->onGoal(waypoints.back());
+ globalPlan = config.stack.globalPlanner->plan(waypoints.back());
+
+ ARMARX_TRACE;
+
+ if (not globalPlan.has_value())
+ {
+ ARMARX_WARNING << "No global trajectory. Cannot move.";
+ srv.publisher->globalPlanningFailed(
+ core::GlobalPlanningFailedEvent{{.timestamp = armarx::Clock::Now()}, {""}});
+
+ srv.introspector->failure();
+ return;
+ }
+
+ ARMARX_TRACE;
+ srv.publisher->globalTrajectoryUpdated(globalPlan.value());
+ srv.introspector->onGlobalPlannerResult(globalPlan.value());
+ // srv.executor->execute(globalPlan->trajectory);
ARMARX_INFO << "Global planning completed. Will now start all required threads";
ARMARX_TRACE;
@@ -721,6 +806,20 @@ namespace armarx::navigation::server
{
}
+ void
+ Navigator::moveToLocation(const std::string& location)
+ {
+ const auto resolveLocation = [&](const std::string& location)
+ {
+ const auto locations = srv.sceneProvider->scene().staticScene->locations;
+ ARMARX_CHECK(locations.count(location) > 0) << "Unknown location `" << location << "`.";
+ return locations.at(location);
+ };
+
+ const core::Pose global_T_location = resolveLocation(location);
+ moveToAbsolute({global_T_location});
+ }
+
void
Navigator::moveTowardsAbsolute(const core::Direction& direction)
{
@@ -739,8 +838,79 @@ namespace armarx::navigation::server
const Duration duration =
armarx::core::time::StopWatch::measure([&]() { updateScene(); });
- ARMARX_VERBOSE << deactivateSpam(0.2) << "Scene update: " << duration.toMilliSecondsDouble()
- << "ms.";
+ ARMARX_DEBUG << deactivateSpam(0.2)
+ << "Scene update: " << duration.toMilliSecondsDouble() << "ms.";
+
+ srv.debugObserverHelper->setDebugObserverDatafield("scene update [ms]",
+ duration.toMilliSecondsDouble());
+ }
+
+ // eventually, draw
+ if(srv.introspector and srv.sceneProvider and srv.sceneProvider->scene().robot)
+ {
+ srv.introspector->onRobotPose(core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()));
+ }
+
+ // global planner update if goal has changed
+ {
+ std::lock_guard g{globalPlanningRequestMtx};
+
+ if (globalPlanningRequest.has_value())
+ {
+ const auto& waypoints = globalPlanningRequest.value();
+
+ // recreate goal monitor
+ {
+ // first we check if we are already at the goal position
+ goalReachedMonitor = std::nullopt;
+ goalReachedMonitor = GoalReachedMonitor(
+ waypoints.back(), srv.sceneProvider->scene(), GoalReachedMonitorConfig());
+
+ if (goalReachedMonitor->goalReached())
+ {
+ ARMARX_INFO << "Already at goal position. Robot won't move.";
+
+ srv.publisher->goalReached(core::GoalReachedEvent{
+ {armarx::Clock::Now()},
+ core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
+
+ return;
+ }
+ }
+
+ // global planning
+ {
+ // global planner
+ ARMARX_INFO << "Update/Planning global trajectory";
+ ARMARX_CHECK_NOT_NULL(config.stack.globalPlanner);
+ // TODO plan on multiple waypoints, ignoring waypoints for now
+ // idea: compute multiple global trajectories, one for each segment between waypoints.
+
+ srv.introspector->onGoal(waypoints.back());
+ globalPlan = config.stack.globalPlanner->plan(waypoints.back());
+
+ ARMARX_TRACE;
+
+ if (not globalPlan.has_value())
+ {
+ ARMARX_WARNING << "No global trajectory. Cannot move.";
+ srv.publisher->globalPlanningFailed(core::GlobalPlanningFailedEvent{
+ {.timestamp = armarx::Clock::Now()}, {""}});
+
+ srv.introspector->failure();
+ return;
+ }
+
+ ARMARX_TRACE;
+ srv.publisher->globalTrajectoryUpdated(globalPlan.value());
+ srv.introspector->onGlobalPlannerResult(globalPlan.value());
+
+ if(not hasLocalPlanner())
+ {
+ updateExecutor(globalPlan.value());
+ }
+ }
+ }
}
// local planner update
@@ -757,8 +927,11 @@ namespace armarx::navigation::server
updateIntrospector(localPlannerResult);
}
});
- ARMARX_VERBOSE << deactivateSpam(0.2)
- << "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
+ ARMARX_DEBUG << deactivateSpam(0.2)
+ << "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
+
+ srv.debugObserverHelper->setDebugObserverDatafield("local planner update [ms]",
+ duration.toMilliSecondsDouble());
}
// monitor update
@@ -768,8 +941,11 @@ namespace armarx::navigation::server
const Duration duration =
armarx::core::time::StopWatch::measure([&]() { updateMonitor(); });
- ARMARX_VERBOSE << deactivateSpam(0.2)
- << "Monitor update: " << duration.toMilliSecondsDouble() << "ms.";
+ ARMARX_DEBUG << deactivateSpam(0.2)
+ << "Monitor update: " << duration.toMilliSecondsDouble() << "ms.";
+
+ srv.debugObserverHelper->setDebugObserverDatafield("monitor update [ms]",
+ duration.toMilliSecondsDouble());
}
}
@@ -780,28 +956,46 @@ namespace armarx::navigation::server
srv.sceneProvider->synchronize(armarx::Clock::Now());
}
- std::optional<loc_plan::LocalPlannerResult>
+ std::optional<local_planning::LocalPlannerResult>
Navigator::updateLocalPlanner()
{
ARMARX_CHECK(hasLocalPlanner());
- localPlan = config.stack.localPlanner->plan(globalPlan->trajectory);
+ ARMARX_VERBOSE << "Updating local plan";
- 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
- Navigator::updateExecutor(const std::optional<loc_plan::LocalPlannerResult>& localPlan)
+ Navigator::updateExecutor(const std::optional<local_planning::LocalPlannerResult>& localPlan)
{
+ if (srv.executor == nullptr)
+ {
+ return;
+ }
+
+
if (isPaused() or isStopped())
{
// [[unlikely]]
@@ -813,6 +1007,7 @@ namespace armarx::navigation::server
{
ARMARX_INFO << "Local plan is invalid!";
srv.executor->stop();
+ return;
}
@@ -831,11 +1026,41 @@ namespace armarx::navigation::server
// ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
// << robotFrameVelocity.linear;
- srv.executor->execute(localPlan->trajectory);
+ srv.executor->execute(localPlan->trajectory, true);
}
void
- Navigator::updateIntrospector(const std::optional<loc_plan::LocalPlannerResult>& localPlan)
+ Navigator::updateExecutor(const std::optional<global_planning::GlobalPlannerResult>& globalPlan)
+ {
+ if (srv.executor == nullptr)
+ {
+ return;
+ }
+
+ ARMARX_IMPORTANT << "Requested to execute global plan with " << globalPlan->trajectory.points().size() << " points.";
+
+
+ // if (isPaused() or isStopped())
+ // {
+ // // [[unlikely]]
+ // ARMARX_VERBOSE << deactivateSpam(1) << "stopped or paused";
+ // return;
+ // }
+
+ if (not globalPlan.has_value())
+ {
+ ARMARX_INFO << "Global plan is invalid!";
+ srv.executor->stop();
+ return;
+ }
+
+ ARMARX_IMPORTANT << "Executing global plan with " << globalPlan->trajectory.points().size() << " points.";
+ srv.executor->execute(globalPlan->trajectory, false);
+ }
+
+ void
+ Navigator::updateIntrospector(
+ const std::optional<local_planning::LocalPlannerResult>& localPlan)
{
ARMARX_CHECK_NOT_NULL(srv.introspector);
@@ -930,8 +1155,11 @@ namespace armarx::navigation::server
executorEnabled.store(false);
- ARMARX_CHECK_NOT_NULL(srv.executor);
- srv.executor->stop();
+ if (srv.executor != nullptr)
+ {
+ ARMARX_INFO << "Stopping executor.";
+ srv.executor->stop();
+ }
}
void
@@ -941,7 +1169,15 @@ namespace armarx::navigation::server
executorEnabled.store(true);
- srv.executor->start();
+ if (srv.executor != nullptr)
+ {
+ if(hasLocalPlanner())
+ {
+ srv.executor->start(ExecutorInterface::ControllerType::LocalTrajectory);
+ }else {
+ srv.executor->start(ExecutorInterface::ControllerType::GlobalTrajectory);
+ }
+ }
}
void
diff --git a/source/armarx/navigation/server/Navigator.h b/source/armarx/navigation/server/Navigator.h
index a0affd5d1fad6ebbe6787e3132d65939e823266d..82305e8a8a5be5d71e7f9bbb5da1ffed26737438 100644
--- a/source/armarx/navigation/server/Navigator.h
+++ b/source/armarx/navigation/server/Navigator.h
@@ -30,6 +30,7 @@
#include <Eigen/Core>
// ArmarX
+#include "ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
@@ -47,7 +48,7 @@
#include <armarx/navigation/server/execution/ExecutorInterface.h>
#include <armarx/navigation/server/introspection/IntrospectorInterface.h>
#include <armarx/navigation/server/monitoring/GoalReachedMonitor.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::server
{
@@ -81,6 +82,8 @@ namespace armarx::navigation::server
EventPublishingInterface* publisher;
IntrospectorInterface* introspector = nullptr;
+ armarx::DebugObserverHelper* debugObserverHelper;
+
scene_provider::SceneProviderInterface* sceneProvider;
};
@@ -89,6 +92,9 @@ namespace armarx::navigation::server
void moveTo(const std::vector<core::Pose>& waypoints,
core::NavigationFrame navigationFrame) override;
+ void update(const std::vector<core::Pose>& waypoints,
+ core::NavigationFrame navigationFrame) override;
+
void moveTo(const std::vector<client::WaypointTarget>& targets,
core::NavigationFrame navigationFrame) override;
@@ -96,6 +102,8 @@ namespace armarx::navigation::server
void moveTowards(const core::Direction& direction,
core::NavigationFrame navigationFrame) override;
+ void moveToLocation(const std::string& location) override;
+
void pause() override;
void resume() override;
@@ -116,13 +124,16 @@ namespace armarx::navigation::server
void moveToAbsolute(const std::vector<core::Pose>& waypoints);
void moveTowardsAbsolute(const core::Direction& direction);
+ void updateAbsolute(const std::vector<core::Pose>& waypoints);
+
void run();
void updateScene();
- std::optional<loc_plan::LocalPlannerResult> updateLocalPlanner();
- void updateExecutor(const std::optional<loc_plan::LocalPlannerResult>& localPlan);
- void updateIntrospector(const std::optional<loc_plan::LocalPlannerResult>& localPlan);
+ std::optional<local_planning::LocalPlannerResult> updateLocalPlanner();
+ void updateExecutor(const std::optional<local_planning::LocalPlannerResult>& localPlan);
+ void updateExecutor(const std::optional<global_planning::GlobalPlannerResult>& localPlan);
+ void updateIntrospector(const std::optional<local_planning::LocalPlannerResult>& localPlan);
void updateMonitor();
@@ -137,7 +148,8 @@ namespace armarx::navigation::server
void setGraphEdgeCosts(core::Graph& graph) const;
GraphBuilder convertToGraph(const std::vector<client::WaypointTarget>& targets) const;
- bool hasLocalPlanner() const noexcept
+ bool
+ hasLocalPlanner() const noexcept
{
return config.stack.localPlanner != nullptr;
}
@@ -154,7 +166,12 @@ namespace armarx::navigation::server
std::optional<global_planning::GlobalPlannerResult> globalPlan;
- std::optional<loc_plan::LocalPlannerResult> localPlan;
+ std::optional<local_planning::LocalPlannerResult> localPlan;
+
+ using Waypoints = std::vector<core::Pose>;
+
+ std::mutex globalPlanningRequestMtx;
+ std::optional<Waypoints> globalPlanningRequest;
};
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/StackResult.h b/source/armarx/navigation/server/StackResult.h
index c7f84cd3577c0c1ea3e7cae2cee2026a7a2323f7..47fe1bf6e545490690eefa8f76a7a398be1b8ad8 100644
--- a/source/armarx/navigation/server/StackResult.h
+++ b/source/armarx/navigation/server/StackResult.h
@@ -24,9 +24,11 @@
#include <optional>
+#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::server
@@ -35,13 +37,12 @@ namespace armarx::navigation::server
struct StackResult
{
// TODO make struct, add timestamp
- using LocalPlannerResult = core::TrajectoryPtr;
using SafetyControllerResult = std::optional<core::Twist>;
global_planning::GlobalPlannerResult globalPlan;
- LocalPlannerResult localTrajectory;
- traj_ctrl::TrajectoryControllerResult controlVelocity;
- SafetyControllerResult safeVelocity;
+ local_planning::LocalPlannerResult localTrajectory;
+ // traj_ctrl::global::TrajectoryControllerResult controlVelocity;
+ // SafetyControllerResult safeVelocity;
// core::TrajectoryPtr trajectory() const;
// core::Twist velocity() const;
diff --git a/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h b/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
index 416d8cc61155b02144932de10002d803f7c87788..c9cf86ac8e6d89fdd0da20fd5b7ab6f600524207 100644
--- a/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
+++ b/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
@@ -4,7 +4,7 @@
#include <armarx/navigation/core/events.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::server
@@ -19,11 +19,12 @@ namespace armarx::navigation::server
public:
// TODO(fabian.reister): fwd
virtual void globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& res) = 0;
- virtual void localTrajectoryUpdated(const loc_plan::LocalPlannerResult& res) = 0;
- virtual void
- trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& res) = 0;
+ virtual void localTrajectoryUpdated(const local_planning::LocalPlannerResult& res) = 0;
+ // virtual void
+ // 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 6226169b07d7b9f6daf5d38aa7b584ba90748df4..6c74049dc1136bb2c192b60fa1b130e1bca64416 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
@@ -52,23 +52,29 @@ namespace armarx::navigation::server
}
void
- MemoryPublisher::localTrajectoryUpdated(const loc_plan::LocalPlannerResult& res)
- {
- // resultWriter->store(res, clientId);
- }
-
- void
- MemoryPublisher::trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& res)
+ MemoryPublisher::localTrajectoryUpdated(const local_planning::LocalPlannerResult& res)
{
resultWriter->store(res, clientId);
}
+ // void
+ // MemoryPublisher::trajectoryControllerUpdated(
+ // const traj_ctrl::local::TrajectoryControllerResult& res)
+ // {
+ // resultWriter->store(res, clientId);
+ // }
+
// TODO(fabian.reister): event with message or reason (enum)
void
MemoryPublisher::globalPlanningFailed(const core::GlobalPlanningFailedEvent& event)
{
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 57204aba218df6073fd5d36359e4fbfa4079ba6e..edf5ff0d3a7908657821ed4a465ce268f81af08c 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
@@ -17,10 +17,13 @@ namespace armarx::navigation::server
const std::string& clientId);
void globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& res) override;
- void localTrajectoryUpdated(const loc_plan::LocalPlannerResult& res) override;
- void trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& res) override;
+ void localTrajectoryUpdated(const local_planning::LocalPlannerResult& res) override;
+ // void trajectoryControllerUpdated(
+ // 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;
@@ -36,7 +39,6 @@ namespace armarx::navigation::server
void internalError(const core::InternalErrorEvent& event) override;
// Non-API.
- public:
~MemoryPublisher() override = default;
private:
diff --git a/source/armarx/navigation/server/execution/DummyExecutor.h b/source/armarx/navigation/server/execution/DummyExecutor.h
index 3a9d25a9fc19645719a5a1df42b7c2587cf2618f..4222cda1402cb64ff895d3a3e51368070fc0dded 100644
--- a/source/armarx/navigation/server/execution/DummyExecutor.h
+++ b/source/armarx/navigation/server/execution/DummyExecutor.h
@@ -1,15 +1,11 @@
#pragma once
-// STD/STL
#include <chrono>
-// Eigen
#include <Eigen/Geometry>
-// Simox
#include <VirtualRobot/Robot.h>
-// Navigation
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/server/execution/ExecutorInterface.h>
@@ -20,20 +16,12 @@ namespace armarx::navigation::server
{
public:
- struct Params
- {
- };
-
- DummyExecutor(const VirtualRobot::RobotPtr& robot, const Params& params) : robot(robot)
- {
- }
+ DummyExecutor() = default;
- ~DummyExecutor() override
- {
- }
+ ~DummyExecutor() override = default;
void
- execute(const core::Trajectory& trajectory) override
+ execute(const core::LocalTrajectory& trajectory) override
{
}
diff --git a/source/armarx/navigation/server/execution/ExecutorInterface.h b/source/armarx/navigation/server/execution/ExecutorInterface.h
index ea77fd7a6d2c41bd5ec7bc375a37cba1c2d1ec57..2d8d7a282abd09923f00ae4191a6d95a0326e51e 100644
--- a/source/armarx/navigation/server/execution/ExecutorInterface.h
+++ b/source/armarx/navigation/server/execution/ExecutorInterface.h
@@ -1,9 +1,6 @@
#pragma once
-namespace armarx::navigation::core
-{
- class Trajectory;
-} // namespace armarx::navigation::core
+#include <armarx/navigation/core/Trajectory.h>
namespace armarx::navigation::server
{
@@ -17,10 +14,20 @@ namespace armarx::navigation::server
public:
virtual ~ExecutorInterface() = default;
- virtual void execute(const core::Trajectory& trajectory) = 0;
-
- virtual void start() = 0;
+ virtual void execute(const core::LocalTrajectory& trajectory, bool activateController = false) = 0;
+ virtual void execute(const core::GlobalTrajectory& trajectory, bool activateController = false) = 0;
+
+
+ enum class ControllerType
+ {
+ GlobalTrajectory,
+ LocalTrajectory,
+ None
+ };
+
+ virtual void start(ControllerType controllerType) = 0;
virtual void stop() = 0;
+
};
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
index 5f089038f82c2b6b3e70a4ea20755fe824a45654..57569aa46345e8cead4fbeeab779c4ace4364cd7 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -6,12 +6,14 @@
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/control/common/type.h>
#include <armarx/control/memory/config/util.h>
+
#include <armarx/navigation/common/controller_types.h>
#include <armarx/navigation/core/aron_conversions.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/aron_conversions.h>
#include <armarx/navigation/platform_controller/controller_descriptions.h>
#include <armarx/navigation/platform_controller/json_conversions.h>
+#include <armarx/navigation/trajectory_control/local/aron_conversions.h>
namespace armarx::navigation::server
@@ -19,7 +21,7 @@ namespace armarx::navigation::server
PlatformControllerExecutor::PlatformControllerExecutor(
ControllerComponentPlugin& controllerComponentPlugin) :
- controllerPlugin(controllerComponentPlugin)
+ controllerPlugin_(controllerComponentPlugin)
{
ARMARX_TRACE;
controllerComponentPlugin.getRobotUnitPlugin().getRobotUnit()->loadLibFromPackage(
@@ -37,32 +39,51 @@ namespace armarx::navigation::server
// "" /*configBasePath*/, controllerComponentPlugin.configMemoryWriter());
ARMARX_TRACE;
- ARMARX_INFO << "asdlfasfdlh";
}
// initialize controller
- ARMARX_INFO << "Initializing controller";
+ ARMARX_INFO << "Initializing local trajectory controller";
+ {
+ ARMARX_TRACE;
+ auto builder = controllerPlugin_.createControllerBuilder<
+ armarx::navigation::common::ControllerType::PlatformLocalTrajectory>();
+
+ ARMARX_TRACE;
+
+ const armarx::PackagePath configPath(
+ "armarx_navigation", "controller_config/PlatformTrajectory/default.json");
+
+ auto ctrlWrapper = builder.withNodeSet("PlatformPlanning")
+ .withConfig(configPath.toSystemPath())
+ .create();
+
+ ARMARX_TRACE;
+ ARMARX_CHECK(ctrlWrapper.has_value());
+ localTrajCtrl_.emplace(std::move(ctrlWrapper.value()));
+ }
+
+ ARMARX_INFO << "Initializing global trajectory controller";
{
ARMARX_TRACE;
- auto builder = controllerPlugin.createControllerBuilder<
- armarx::navigation::common::ControllerType::PlatformTrajectory>();
+ auto builder = controllerPlugin_.createControllerBuilder<
+ armarx::navigation::common::ControllerType::PlatformGlobalTrajectory>();
ARMARX_TRACE;
- const armarx::PackagePath configPath("armarx_navigation", "controller_config/PlatformTrajectory/default.json");
+ const armarx::PackagePath configPath("armarx_navigation",
+ "controller_config/GlobalTrajectory/default.json");
- auto ctrlWrapper =
- builder.withNodeSet("PlatformPlanning")
- .withConfig(configPath.toSystemPath())
- .create();
+ auto ctrlWrapper = builder.withNodeSet("PlatformPlanning")
+ .withConfig(configPath.toSystemPath())
+ .create();
ARMARX_TRACE;
ARMARX_CHECK(ctrlWrapper.has_value());
- ctrl.emplace(std::move(ctrlWrapper.value()));
+ globalTrajCtrl_.emplace(std::move(ctrlWrapper.value()));
}
ARMARX_TRACE;
- ARMARX_CHECK(ctrl.has_value());
+ ARMARX_CHECK(localTrajCtrl_.has_value());
ARMARX_INFO << "PlatformControllerExecutor: init done.";
}
@@ -70,27 +91,85 @@ namespace armarx::navigation::server
void
- PlatformControllerExecutor::execute(const core::Trajectory& trajectory)
+ PlatformControllerExecutor::execute(const core::LocalTrajectory& trajectory, const bool activateController)
+ {
+ ARMARX_VERBOSE << "Received trajectory for execution with " << trajectory.points().size()
+ << " points";
+
+ toAron(localTrajCtrl_->config.targets.trajectory, trajectory);
+
+ // sends the updated config to the controller and stores it in the memory
+ localTrajCtrl_->updateConfig();
+
+ if (activateController and not localTrajCtrl_->ctrl()->isControllerActive())
+ {
+ localTrajCtrl_->activate();
+ }
+ }
+
+ void
+ PlatformControllerExecutor::execute(const core::GlobalTrajectory& trajectory, const bool activateController)
{
- ARMARX_INFO << "Received trajectory for execution with " << trajectory.points().size()
- << " points";
+ ARMARX_VERBOSE << "Received trajectory for execution with " << trajectory.points().size()
+ << " points";
+
+ toAron(globalTrajCtrl_->config.targets.trajectory, trajectory);
- toAron(ctrl->config.targets.trajectory, trajectory);
+ ARMARX_INFO << "Sending trajectory with "
+ << globalTrajCtrl_->config.targets.trajectory.points.size()
+ << " to controller.";
// sends the updated config to the controller and stores it in the memory
- ctrl->updateConfig();
+ globalTrajCtrl_->updateConfig();
+
+ if (activateController and not globalTrajCtrl_->ctrl()->isControllerActive())
+ {
+ globalTrajCtrl_->activate();
+ }
}
+
void
- PlatformControllerExecutor::start()
+ PlatformControllerExecutor::start(const ControllerType controllerType)
{
- // TODO check whether the controller must be resetted (trajectory)
- ctrl->activate();
+ switch (controllerType)
+ {
+ case ControllerType::GlobalTrajectory:
+ if (globalTrajCtrl_.has_value())
+ {
+ globalTrajCtrl_->activate();
+ }
+ break;
+ case ControllerType::LocalTrajectory:
+ if (localTrajCtrl_.has_value())
+ {
+ localTrajCtrl_->activate();
+ }
+ break;
+ case ControllerType::None:
+ ARMARX_INFO << "No controller selected.";
+ break;
+ }
}
+
void
PlatformControllerExecutor::stop()
{
- ctrl->deactivate();
+ lastActiveController_ = ControllerType::None;
+
+ if (localTrajCtrl_->ctrl()->isControllerActive())
+ {
+ ARMARX_INFO << "Stopping local trajectory controller.";
+ localTrajCtrl_->deactivate();
+ lastActiveController_ = ControllerType::LocalTrajectory;
+ }
+
+ if (globalTrajCtrl_->ctrl()->isControllerActive())
+ {
+ ARMARX_INFO << "Stopping global trajectory controller.";
+ globalTrajCtrl_->deactivate();
+ lastActiveController_ = ControllerType::GlobalTrajectory;
+ }
}
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
index d256904cefa0cee344faeea3bddc4b9197f70c60..2b1641a4d13b30c9d25f1f521e34eabbe4241ed6 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
@@ -5,7 +5,6 @@
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/navigation/common/controller_types.h>
#include <armarx/navigation/core/types.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/controller_descriptions.h>
#include <armarx/navigation/server/execution/ExecutorInterface.h>
@@ -32,17 +31,28 @@ namespace armarx::navigation::server
PlatformControllerExecutor(ControllerComponentPlugin& controllerComponentPlugin);
~PlatformControllerExecutor() override;
- void execute(const core::Trajectory& trajectory) override;
+ void execute(const core::LocalTrajectory& trajectory, bool activateController = false) override;
+ void execute(const core::GlobalTrajectory& trajectory, bool activateController = false) override;
+
+ void start(ControllerType controllerType) override;
+
- void start() override;
void stop() override;
private:
std::optional<armarx::control::client::ControllerWrapper<
- armarx::navigation::common::ControllerType::PlatformTrajectory>>
- ctrl;
+ armarx::navigation::common::ControllerType::PlatformLocalTrajectory>>
+ localTrajCtrl_;
+
+ std::optional<armarx::control::client::ControllerWrapper<
+ armarx::navigation::common::ControllerType::PlatformGlobalTrajectory>>
+ globalTrajCtrl_;
+
+ ControllerComponentPlugin& controllerPlugin_;
+
+
- ControllerComponentPlugin& controllerPlugin;
+ ControllerType lastActiveController_ = ControllerType::None;
};
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index 0f107c9907a91ef08c2ef99ba3b4c4c0b2330491..89a0a92da7100a01e33d7fa58551d286c2622ae9 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -3,26 +3,29 @@
#include <iterator>
#include <string>
-#include <range/v3/view/enumerate.hpp>
-
#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>
#include <VirtualRobot/Robot.h>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
#include <RobotAPI/components/ArViz/Client/elements/Path.h>
+#include <armarx/navigation/core/Trajectory.h>
#include "range/v3/algorithm/max.hpp"
#include "range/v3/algorithm/max_element.hpp"
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/server/StackResult.h>
#include <armarx/navigation/util/Visualization.h>
+#include <range/v3/view/enumerate.hpp>
namespace armarx::navigation::server
{
@@ -40,23 +43,20 @@ namespace armarx::navigation::server
void
ArvizIntrospector::onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result)
{
- std::lock_guard g{mtx};
-
ARMARX_DEBUG << "ArvizIntrospector::onGlobalPlannerResult";
drawGlobalTrajectory(result.trajectory);
- arviz.commit(layers);
+ arviz.commit(simox::alg::get_values(layers));
visualization.visualize(result.trajectory);
}
void
- ArvizIntrospector::onLocalPlannerResult(const loc_plan::LocalPlannerResult& result)
+ ArvizIntrospector::onLocalPlannerResult(const local_planning::LocalPlannerResult& result)
{
- std::lock_guard g{mtx};
-
drawLocalTrajectory(result.trajectory);
+ arviz.commit(simox::alg::get_values(layers));
}
// void
@@ -79,20 +79,43 @@ namespace armarx::navigation::server
void
ArvizIntrospector::onGoal(const core::Pose& goal)
{
- std::lock_guard g{mtx};
-
auto layer = arviz.layer("goal");
layer.add(viz::Pose("goal").pose(goal).scale(3));
- arviz.commit(layer);
+
+ robotPosesLayer.clear();
+
+ arviz.commit({layer, robotPosesLayer});
visualization.setTarget(goal);
}
+ void ArvizIntrospector::onRobotPose(const core::Pose& pose)
+ {
+
+ constexpr float eps = 50; // [mm]
+
+ if(not lastPose)
+ {
+ lastPose = pose;
+ }
+ else {
+ // only draw poses every `eps` mm
+ if((lastPose->translation() - pose.translation()).norm() < eps)
+ {
+ return;
+ }
+
+ lastPose = pose;
+ }
+
+ robotPosesLayer.add(viz::Pose("pose" + std::to_string(robotPosesLayer.size())).pose(pose));
+ arviz.commit(robotPosesLayer);
+ }
+
+
void
ArvizIntrospector::onGlobalShortestPath(const std::vector<core::Pose>& path)
{
- std::lock_guard g{mtx};
-
auto layer = arviz.layer("graph_shortest_path");
const auto toPosition = [](const core::Pose& pose) -> core::Position
@@ -120,7 +143,7 @@ namespace armarx::navigation::server
// private methods
void
- ArvizIntrospector::drawGlobalTrajectory(const core::Trajectory& trajectory)
+ ArvizIntrospector::drawGlobalTrajectory(const core::GlobalTrajectory& trajectory)
{
auto layer = arviz.layer("global_planner");
@@ -128,10 +151,11 @@ namespace armarx::navigation::server
viz::Path("path").points(trajectory.positions()).color(simox::color::Color::blue()));
const auto cmap = simox::color::cmaps::viridis();
-
- const float maxVelocity = ranges::max_element(trajectory.points(), [](const auto& a, const auto& b){
- return a.velocity < b.velocity;
- })->velocity;
+
+ const float maxVelocity = ranges::max_element(trajectory.points(),
+ [](const auto& a, const auto& b)
+ { return a.velocity < b.velocity; })
+ ->velocity;
for (const auto& [idx, tp] : trajectory.points() | ranges::views::enumerate)
@@ -147,17 +171,47 @@ namespace armarx::navigation::server
.color(cmap.at(tp.velocity / maxVelocity)));
}
- layers.emplace_back(std::move(layer));
+ layers[layer.data_.name] = std::move(layer);
}
void
- ArvizIntrospector::drawLocalTrajectory(const core::Trajectory& trajectory)
+ ArvizIntrospector::drawLocalTrajectory(const core::LocalTrajectory& trajectory)
{
auto layer = arviz.layer("local_planner");
- layer.add(viz::Path("path").points(trajectory.positions()).color(simox::Color::blue()));
+ const std::vector<Eigen::Vector3f> points =
+ simox::alg::apply(trajectory.points(),
+ [](const core::LocalTrajectoryPoint& pt) -> Eigen::Vector3f
+ { return pt.pose.translation(); });
+
+ layer.add(viz::Path("path").points(points).color(simox::Color::green()));
+
+
+ // Visualize trajectory speed
+ auto velLayer = arviz.layer("local_planner_velocity");
+
+ simox::ColorMap cm = simox::color::cmaps::inferno();
+ cm.set_vmin(0);
+ cm.set_vmax(0.6);
+
+ for (size_t i = 0; i < trajectory.points().size() - 1; i++)
+ {
+ const core::LocalTrajectoryPoint start = trajectory.points().at(i);
+ const core::LocalTrajectoryPoint end = trajectory.points().at(i + 1);
+
+ const Duration dT = end.timestamp - start.timestamp;
+ const Eigen::Vector3f distance = end.pose.translation() - start.pose.translation();
+ const float speed = distance.norm() / 1000 / dT.toSecondsDouble();
- layers.emplace_back(std::move(layer));
+ const Eigen::Vector3f pos = start.pose.translation() + distance / 2;
+ const simox::Color color = cm.at(speed);
+
+ velLayer.add(
+ viz::Sphere("velocity_" + std::to_string(i)).position(pos).radius(50).color(color));
+ }
+
+ layers[layer.data_.name] = std::move(layer);
+ layers[velLayer.data_.name] = std::move(velLayer);
}
void
@@ -170,7 +224,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
@@ -183,7 +237,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 :
@@ -194,6 +248,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
{
@@ -236,11 +306,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 a3daddc48d80315c081f1d3169bf251824d1b69c..3b2865150d485e2fa656b1a531d3090a6543317b 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -30,17 +30,18 @@
#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
#include "IntrospectorInterface.h"
+#include "armarx/navigation/core/basic_types.h"
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/safety_control/SafetyController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
#include <armarx/navigation/util/Visualization.h>
// forward declaration
namespace armarx::navigation::core
{
- struct Trajectory;
+ class GlobalTrajectory;
}
namespace armarx::navigation::server
@@ -53,12 +54,14 @@ namespace armarx::navigation::server
~ArvizIntrospector() override = default;
void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
- void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) override;
+ void onLocalPlannerResult(const local_planning::LocalPlannerResult& result) override;
+
+ void onRobotPose(const core::Pose& pose) override;
void onGoal(const core::Pose& goal) override;
- void success();
- void failure();
+ void success() override;
+ void failure() override;
void onGlobalShortestPath(const std::vector<core::Pose>& path) override;
@@ -69,20 +72,24 @@ namespace armarx::navigation::server
ArvizIntrospector(ArvizIntrospector&& other) noexcept;
ArvizIntrospector& operator=(ArvizIntrospector&&) noexcept;
+ void clear();
+
private:
- void drawGlobalTrajectory(const core::Trajectory& trajectory);
- void drawLocalTrajectory(const core::Trajectory& trajectory);
+ void drawGlobalTrajectory(const core::GlobalTrajectory& trajectory);
+ void drawLocalTrajectory(const core::LocalTrajectory& trajectory);
void drawRawVelocity(const core::Twist& twist);
void drawSafeVelocity(const core::Twist& twist);
viz::ScopedClient arviz;
const VirtualRobot::RobotPtr robot;
- std::vector<viz::Layer> layers;
-
- std::mutex mtx;
+ std::map<std::string, viz::Layer> layers;
util::Visualization visualization;
+
+ std::optional<core::Pose> lastPose;
+
+ viz::Layer robotPosesLayer;
};
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/introspection/IntrospectorInterface.h b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
index 18eaa36bc1368345ae3d73add730566c17883203..1b40320341548f2cdbd00119d1f6af282116d735 100644
--- a/source/armarx/navigation/server/introspection/IntrospectorInterface.h
+++ b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
@@ -25,7 +25,7 @@
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/safety_control/SafetyController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::server
{
@@ -43,7 +43,9 @@ namespace armarx::navigation::server
virtual void failure() = 0;
virtual void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) = 0;
- virtual void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) = 0;
+ virtual void onLocalPlannerResult(const local_planning::LocalPlannerResult& result) = 0;
+
+ virtual void onRobotPose(const core::Pose& pose) = 0;
virtual void onGlobalShortestPath(const std::vector<core::Pose>& path) = 0;
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
index aaf84a09d75d103ba358908b44326d25a300f6ac..c0b97ecf11fa7043030dee4d50c9df570e1e83c4 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
@@ -17,7 +17,7 @@ namespace armarx::navigation::server
}
void
- MemoryIntrospector::onLocalPlannerResult(const loc_plan::LocalPlannerResult& result)
+ MemoryIntrospector::onLocalPlannerResult(const local_planning::LocalPlannerResult& result)
{
// TODO(fabian.reister): implement
}
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.h b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
index 9d473a7457777cec2601128846af65b343dc4a4b..55b5c4f91b35c717d15536e11fbaa3dcc872d5ec 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.h
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
@@ -39,7 +39,9 @@ namespace armarx::navigation::server
void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
- void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) override;
+ void onLocalPlannerResult(const local_planning::LocalPlannerResult& result) override;
+
+ void onRobotPose(const core::Pose& pose) override {}
void onGoal(const core::Pose& goal) override;
diff --git a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h
index 438c18daa85433c511eb03f3cfc086ee484a30aa..ddfdc0bc96ef6b193045722c1534287f007aea07 100644
--- a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h
+++ b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h
@@ -33,8 +33,8 @@ namespace armarx::navigation::server
struct GoalReachedMonitorConfig
{
- float posTh{30.F}; // [mm]
- float oriTh{VirtualRobot::MathTools::deg2rad(2.F)}; // [rad]
+ float posTh{70.F}; // [mm]
+ 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 74bce9568dbf67ad6f73fea0b116ee006fe0445d..f97c3859ce58f0fe1a387bef6458e01717c7a28d 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
@@ -1,11 +1,22 @@
#include "SceneProvider.h"
#include <VirtualRobot/SceneObjectSet.h>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include "ArmarXCore/core/time/Duration.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/Clock.h>
+
+#include <RobotAPI/libraries/armem_robot/types.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/memory/client/graph/Reader.h>
+#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/util/util.h>
+
namespace armarx::navigation::server::scene_provider
{
@@ -28,11 +39,11 @@ namespace armarx::navigation::server::scene_provider
scn.robot = srv.virtualRobotReader->getRobot(
config.robotName, timestamp, VirtualRobot::RobotIO::RobotDescription::eCollisionModel);
scn.robot->setPrimitiveApproximationModel({"navigation"});
-
+
ARMARX_CHECK_NOT_NULL(scn.robot);
- scn.staticScene = getStaticScene(timestamp);
- scn.dynamicScene = getDynamicScene(timestamp);
+ scn.staticScene.emplace(getStaticScene(timestamp));
+ scn.dynamicScene.emplace(getDynamicScene(timestamp));
scn.graph = getSceneGraph(timestamp);
return true; // TODO(fabian.reister): return false if sync fails
@@ -49,6 +60,13 @@ namespace armarx::navigation::server::scene_provider
scn.dynamicScene = getDynamicScene(timestamp);
// scn.graph = getSceneGraph(timestamp);
+ const auto platformState = srv.virtualRobotReader->queryPlatformState(
+ armem::robot::RobotDescription{.name = scn.robot->getName()}, timestamp);
+ ARMARX_CHECK(platformState.has_value());
+
+ scn.platformVelocity = core::Twist{.linear = platformState->twist.linear,
+ .angular = platformState->twist.angular};
+
return true; // TODO(fabian.reister): return false if sync fails
}
@@ -68,34 +86,49 @@ namespace armarx::navigation::server::scene_provider
ARMARX_CHECK_NOT_NULL(objects);
ARMARX_INFO << objects->getSize() << " objects in the scene";
- ARMARX_INFO << "Creating costmap";
- ARMARX_CHECK_NOT_NULL(scn.robot);
+ ARMARX_INFO << "Retrieving costmap in memory";
- // FIXME: move costmap creation out of this component
- // FIXME create costmap writer enum: type of costmaps
- algorithms::CostmapBuilder costmapBuilder(
- scn.robot,
- objects,
- algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
- "Platform-navigation-colmodel");
+ ARMARX_TRACE;
- const auto costmap = costmapBuilder.create();
+ const algorithms::Costmap costmap = [&]()
+ {
+ // 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()};
- // ARMARX_INFO << "Storing costmap in memory";
- // costmapWriterPlugin->get().store(
- // costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
- ARMARX_INFO << "Done";
+ 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();
+ }
- ARMARX_TRACE;
+ ARMARX_INFO << deactivateSpam(5) << "Static costmap `" << query.name
+ << "` from provider " << query.providerName << " not available yet.";
+ }
+ }();
- return {.objects = objects, .costmap = std::make_unique<algorithms::Costmap>(costmap)};
+ const auto locations = srv.graphReader->locations();
+
+ ARMARX_INFO << "Retrieved static costmap";
+
+ return {.objects = objects, .distanceToObstaclesCostmap = costmap, .locations = locations};
}
core::DynamicScene
SceneProvider::getDynamicScene(const DateTime& timestamp) const
{
- return {}; // FIXME implement
+ const memory::client::human::Reader::Query query{.providerName = config.humanProviderName,
+ .timestamp = timestamp,
+ .maxAge = Duration::MilliSeconds(500)};
+
+ return {.humans = srv.humanReader->queryHumans(query).humans};
}
core::SceneGraph
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.h b/source/armarx/navigation/server/scene_provider/SceneProvider.h
index cd93e072306246b8dc5a50dc58cee43459e173bf..1f21e85ead9be31ca561a925e2bcceff0be53567 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.h
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.h
@@ -31,12 +31,28 @@
#include <armarx/navigation/core/DynamicScene.h>
#include <armarx/navigation/core/StaticScene.h>
-#include <armarx/navigation/memory/client/costmap/Reader.h>
-#include <armarx/navigation/memory/client/graph/Reader.h>
#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/server/scene_provider/SceneProviderInterface.h>
+namespace armarx::navigation::memory::client
+{
+ namespace graph
+ {
+ class Reader;
+ }
+ namespace costmap
+ {
+ class Reader;
+ }
+ namespace human
+ {
+ class Reader;
+ }
+} // namespace armarx::navigation::memory::client
+
+
namespace armarx::navigation::server::scene_provider
{
@@ -54,12 +70,19 @@ namespace armarx::navigation::server::scene_provider
// `robot_state` memory reader and writer
armem::robot_state::VirtualRobotReader* virtualRobotReader;
+ memory::client::human::Reader* humanReader;
+
objpose::ObjectPoseClient objectPoseClient;
};
struct Config
{
std::string robotName;
+
+ std::string staticCostmapProviderName = "distance_to_obstacle_costmap_provider";
+ std::string staticCostmapName = "distance_to_obstacles";
+
+ std::string humanProviderName = "dynamic_scene_provider";
};
SceneProvider(const InjectedServices& srv, const Config& config);
diff --git a/source/armarx/navigation/server/test/serverTest.cpp b/source/armarx/navigation/server/test/serverTest.cpp
index d7ba7e7f2643f33365b14c8554034ab36ed02bb5..484209450dce81026cc307592a323371d8d346f5 100644
--- a/source/armarx/navigation/server/test/serverTest.cpp
+++ b/source/armarx/navigation/server/test/serverTest.cpp
@@ -21,7 +21,7 @@
* GNU General Public License
*/
-#include "armarx/navigation/server/scene_provider/SceneProvider.h"
+#include <armarx/navigation/server/scene_provider/SceneProvider.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/services/SimpleEventHandler.h>
#include <armarx/navigation/core/types.h>
@@ -31,7 +31,7 @@
#include <armarx/navigation/server/NavigationStack.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/execution/DummyExecutor.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::server
@@ -56,17 +56,17 @@ namespace armarx::navigation::server::scene_provider
const core::Scene&
scene() const override
{
- return dummyScene;
+ return dummyScene;
};
bool
- initialize(const armarx::DateTime& /*timestamp*/) override
+ initialize(const armarx::DateTime& /*timestamp*/) override
{
return true;
};
bool
- synchronize(const armarx::DateTime& /*timestamp*/) override
+ synchronize(const armarx::DateTime& /*timestamp*/) override
{
return true;
};
@@ -75,7 +75,7 @@ namespace armarx::navigation::server::scene_provider
// non-api
~DummySceneProvider() override = default;
- private:
+ private:
core::Scene dummyScene; // FIXME implement
};
@@ -99,15 +99,17 @@ BOOST_AUTO_TEST_CASE(testNavigator)
// traj_ctrl::TrajectoryFollowingControllerParams(), scene)};
// Executor.
- server::DummyExecutor executor{scene.robot, server::DummyExecutor::Params()};
+ server::DummyExecutor executor;
client::SimpleEventHandler eventHandler;
server::scene_provider::DummySceneProvider sceneProvider;
server::Navigator navigator(
server::Navigator::Config{.stack = stack, .general = server::Navigator::Config::General{}},
- server::Navigator::InjectedServices{
- .executor = &executor, .publisher = &eventHandler, .sceneProvider = &sceneProvider});
+ server::Navigator::InjectedServices{.executor = &executor,
+ .publisher = &eventHandler,
+ .debugObserverHelper = nullptr,
+ .sceneProvider = &sceneProvider});
navigator.moveTo(std::vector{goal}, core::NavigationFrame::Absolute);
BOOST_CHECK_EQUAL(true, true);
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..452d2420e1c79d32a2eb2a6040c231040e9bffca
--- /dev/null
+++ b/source/armarx/navigation/simulation/SimulatedHuman.cpp
@@ -0,0 +1,199 @@
+/**
+ * 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/spfa/ShortestPathFasterAlgorithm.h>
+#include <armarx/navigation/algorithms/util.h>
+#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/core/StaticScene.h>
+#include <armarx/navigation/core/basic_types.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();
+
+ if (goalReached())
+ {
+ 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_;
+ }
+
+ bool
+ SimulatedHuman::goalReached() const
+ {
+ const Eigen::Vector2f goal =
+ conv::to2D(globalTrajectory_.points().back().waypoint.pose.translation());
+
+ return ((goal - human_.pose.translation()).norm() < params_.goalDistanceThreshold);
+ }
+
+ void
+ SimulatedHuman::reinitialize(const core::Pose2D& pose)
+ {
+ human_.pose = pose;
+
+ if(goalReached())
+ {
+ ARMARX_VERBOSE << "Goal reached";
+ state_ = State::Idle;
+ }
+
+ initialize();
+ update();
+
+ }
+
+ void
+ SimulatedHuman::initialize()
+ {
+ global_planning::SPFA::Params params;
+ params.linearVelocity = this->params_.maxLinearVelocity;
+ params.algo.obstacleDistanceWeight = 1.1F;
+
+ core::Scene scene;
+
+ scene.staticScene.emplace(core::StaticScene{nullptr, std::nullopt, {}});
+ scene.staticScene->distanceToObstaclesCostmap.emplace(distanceField_);
+
+ global_planning::SPFA planner(params, scene);
+
+ const core::Pose2D start = human_.pose;
+
+ if (state_ == State::Idle) // human reached goal and needs a new one
+ {
+ while (true)
+ {
+ const auto sampledPose = algorithms::sampleValidPositionInMap(distanceField_);
+ ARMARX_CHECK(sampledPose.has_value());
+ const core::Pose2D& 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;
+ }
+ }
+ }
+ else if (state_ == State::Walking) // human is walking and replans to existing goal
+ {
+ const auto goal = conv::to2D(globalTrajectory_.poses().back());
+ 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();
+
+ update(); // should bring the robot into walking state
+ }
+} // 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..e50bab125ef830ada955ac76d108967ff1bb1b79
--- /dev/null
+++ b/source/armarx/navigation/simulation/SimulatedHuman.h
@@ -0,0 +1,102 @@
+/**
+ * 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; // [mm]
+
+ float minLinearVelocity = 100; // [mm/s]
+ float maxLinearVelocity = 200; // [mm/s]
+ };
+
+ class SimulatedHuman
+ {
+ public:
+ using Params = SimulatedHumanParams;
+
+ SimulatedHuman(const algorithms::Costmap& distanceField, const Params& params = Params());
+
+ Human update();
+
+
+ human::Human&
+ human()
+ {
+ return human_;
+ }
+
+ core::GlobalTrajectory&
+ trajectory()
+ {
+ return globalTrajectory_;
+ }
+
+ const core::GlobalTrajectory&
+ trajectory() const
+ {
+ return globalTrajectory_;
+ }
+
+ bool goalReached() const;
+
+ /**
+ * @brief resets the human to the given pose and replans the global optimal trajectory
+ *
+ * @param pose
+ */
+ void reinitialize(const core::Pose2D& pose);
+
+ 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/components/GraphImportExport/test/GraphImportExportTest.cpp b/source/armarx/navigation/simulation/test/simulationTest.cpp
similarity index 72%
rename from source/armarx/navigation/components/GraphImportExport/test/GraphImportExportTest.cpp
rename to source/armarx/navigation/simulation/test/simulationTest.cpp
index 6be2d4987a355641a761662f24ec473a8d3cbf1d..eb077826955bedc0478271ab6f7e97d2d2c3699c 100644
--- a/source/armarx/navigation/components/GraphImportExport/test/GraphImportExportTest.cpp
+++ b/source/armarx/navigation/simulation/test/simulationTest.cpp
@@ -13,26 +13,24 @@
* 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 MemoryX::ArmarXObjects::GraphImportExport
- * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2021
+ * @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 MemoryX::ArmarXObjects::GraphImportExport
+#define BOOST_TEST_MODULE navigation::ArmarXLibraries::simulation
#define ARMARX_BOOST_TEST
-#include "../GraphImportExport.h"
+#include <navigation/Test.h>
+#include "../simulation.h"
#include <iostream>
-#include <MemoryX/Test.h>
-
BOOST_AUTO_TEST_CASE(testExample)
{
- armarx::GraphImportExport instance;
BOOST_CHECK_EQUAL(true, true);
}
diff --git a/source/armarx/navigation/skills/GuideHumanToRoom.cpp b/source/armarx/navigation/skills/GuideHumanToRoom.cpp
index b9fd5553721245997b97902d8be3fa5ae2bb0d40..0b25333bb3eded6d7bd80d197101b2fc9094b251 100644
--- a/source/armarx/navigation/skills/GuideHumanToRoom.cpp
+++ b/source/armarx/navigation/skills/GuideHumanToRoom.cpp
@@ -5,52 +5,57 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include <Eigen/src/Geometry/AngleAxis.h>
-#include "armarx/navigation/algorithms/Costmap.h"
-#include "armarx/navigation/client/services/MemorySubscriber.h"
-#include "armarx/navigation/core/basic_types.h"
-#include "armarx/navigation/global_planning/SPFA.h"
-#include "armarx/navigation/rooms/RoomNavigationTargetCreator.h"
+#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/client/services/IceNavigator.h>
+#include <armarx/navigation/client/services/MemorySubscriber.h>
#include <armarx/navigation/client/services/SimpleEventHandler.h>
#include <armarx/navigation/client/types.h>
+#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/global_planning/AStar.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/global_planning/SPFA.h>
+#include <armarx/navigation/rooms/RoomNavigationTargetCreator.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
namespace armarx::navigation::skills
{
- GuideHumanToRoom::GuideHumanToRoom(const GuideHumanToRoom::Context& ctx) :
- Base(DefaultSkillDescription()), ctx(ctx)
+ GuideHumanToRoom::GuideHumanToRoom() : Base(DefaultSkillDescription())
{
}
+ void
+ GuideHumanToRoom::connect(const Services& srv)
+ {
+ srv_.emplace(srv);
+ }
+
::armarx::skills::Skill::InitResult
GuideHumanToRoom::init(const Base::SpecializedInitInput& in)
{
+ ARMARX_CHECK(srv_.has_value());
+
// parameterize the navigation stack
client::NavigationStackConfig cfg;
cfg.general(client::GeneralConfig{});
cfg.globalPlanner(armarx::navigation::global_planning::SPFAParams{});
cfg.trajectoryController(
- armarx::navigation::traj_ctrl::TrajectoryFollowingControllerParams{});
+ armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams{});
const std::string configId = DefaultSkillDescription().skillName;
// configure the `navigator` which provides a simplified and typed interface to the navigation server
- memorySubscriber.emplace(configId, ctx.memoryNameSystem);
+ memorySubscriber.emplace(configId, srv_->memoryNameSystem);
// register our config
ARMARX_INFO << "Registering config";
- ctx.iceNavigator.createConfig(cfg, configId);
+ srv_->iceNavigator.createConfig(cfg, configId);
navigator.emplace(client::Navigator::InjectedServices{
- .navigator = &ctx.iceNavigator, .subscriber = &memorySubscriber.value()});
+ .navigator = &srv_->iceNavigator, .subscriber = &memorySubscriber.value()});
return ::armarx::skills::Skill::InitResult{
.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
@@ -126,8 +131,8 @@ namespace armarx::navigation::skills
.name = "distance_to_obstacles",
.timestamp = Clock::Now()};
- const auto result = ctx.costmapReader.query(costmapQuery);
- ARMARX_CHECK(result);
+ const auto result = srv_->costmapReader.query(costmapQuery);
+ ARMARX_CHECK(result) << result.errorMessage;
ARMARX_CHECK(result.costmap.has_value());
return result.costmap.value();
}();
@@ -139,22 +144,26 @@ namespace armarx::navigation::skills
.name = roomName,
.timestamp = Clock::Now()};
- const auto result = ctx.roomsReader.query(roomQuery);
- ARMARX_CHECK(result);
+ const auto result = srv_->roomsReader.query(roomQuery);
+ ARMARX_CHECK(result) << "Failed to query room `" << roomName << "` from provider `"
+ << properties.roomsProvider
+ << "`. Reason: " << result.errorMessage;
ARMARX_CHECK(result.room.has_value());
return result.room.value();
}();
- const VirtualRobot::RobotPtr robot = ctx.virtualRobotReader.getRobot(properties.robotName);
+ const VirtualRobot::RobotPtr robot =
+ srv_->virtualRobotReader.getRobot(properties.robotName);
ARMARX_CHECK_NOT_NULL(robot);
- ARMARX_CHECK(ctx.virtualRobotReader.synchronizeRobot(*robot, Clock::Now()));
+ ARMARX_CHECK(srv_->virtualRobotReader.synchronizeRobot(*robot, Clock::Now()));
const core::Pose global_T_robot(robot->getGlobalPose());
rooms::RoomNavigationTargetCreator::Params algoParams;
rooms::RoomNavigationTargetCreator algo(algoParams);
- auto result = algo.getClosestPositionOutsideOfRoom(costmap, room, global_T_robot.translation());
+ auto result =
+ algo.getClosestPositionOutsideOfRoom(costmap, room, global_T_robot.translation(), &srv_->arviz);
const core::Direction dirToRoom = result.global_P_roomEntry - result.global_P_robot;
@@ -165,7 +174,8 @@ namespace armarx::navigation::skills
core::Pose global_T_robot_target = core::Pose::Identity();
global_T_robot_target.translation() = result.global_P_robot;
- global_T_robot_target.linear() = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+ global_T_robot_target.linear() =
+ Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()).toRotationMatrix();
return global_T_robot_target;
}
diff --git a/source/armarx/navigation/skills/GuideHumanToRoom.h b/source/armarx/navigation/skills/GuideHumanToRoom.h
index 3524f4c48bcb65f788ad557d5875e4fa2c057e5f..ea0f954d2569d6cbc44f36dda53ea483cc3515e6 100644
--- a/source/armarx/navigation/skills/GuideHumanToRoom.h
+++ b/source/armarx/navigation/skills/GuideHumanToRoom.h
@@ -29,8 +29,8 @@
#include <RobotAPI/libraries/skills/provider/Skill.h>
#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
-#include "armarx/navigation/memory/client/costmap/Reader.h"
-#include "armarx/navigation/memory/client/rooms/Reader.h"
+#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/memory/client/rooms/Reader.h>
#include <armarx/navigation/client.h>
#include <armarx/navigation/client/services/MemorySubscriber.h>
#include <armarx/navigation/skills/aron/GuideHumanToRoom.aron.generated.h>
@@ -45,7 +45,7 @@ namespace armarx::navigation::skills
using Base = ::armarx::skills::SpecializedSkill<Params>;
- struct Context
+ struct Services
{
client::IceNavigator& iceNavigator;
armem::client::MemoryNameSystem& memoryNameSystem;
@@ -56,16 +56,21 @@ namespace armarx::navigation::skills
memory::client::costmap::Reader& costmapReader;
memory::client::rooms::Reader& roomsReader;
+
+ viz::Client& arviz;
};
struct Properties
{
- std::string distanceToObstacleCostmapProvider = ""; // FIXME
+ std::string distanceToObstacleCostmapProvider = "distance_to_obstacle_costmap_provider"; // FIXME
std::string roomsProvider = "R003"; // FIXME
std::string robotName = "Armar6"; // FIXME
};
- GuideHumanToRoom(const Context& ctx);
+ GuideHumanToRoom();
+
+ void connect(const Services& srv);
+
private:
::armarx::skills::Skill::InitResult init(const Base::SpecializedInitInput& in) override;
@@ -89,7 +94,7 @@ namespace armarx::navigation::skills
protected:
private:
- const Context ctx;
+ std::optional<Services> srv_;
const Properties properties;
// will be initialized in "init()"
diff --git a/source/armarx/navigation/skills/NavigateTo.cpp b/source/armarx/navigation/skills/NavigateTo.cpp
index 6604e9d3788f6a2c3c4dcc162e19a00fd969fb83..115f6bc557e62252cde283cf9286ff4adc80e8fc 100644
--- a/source/armarx/navigation/skills/NavigateTo.cpp
+++ b/source/armarx/navigation/skills/NavigateTo.cpp
@@ -5,26 +5,30 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include "armarx/navigation/client/services/MemorySubscriber.h"
-#include "armarx/navigation/global_planning/SPFA.h"
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/client/services/IceNavigator.h>
+#include <armarx/navigation/client/services/MemorySubscriber.h>
#include <armarx/navigation/client/services/SimpleEventHandler.h>
#include <armarx/navigation/client/types.h>
#include <armarx/navigation/global_planning/AStar.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/global_planning/SPFA.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
namespace armarx::navigation::skills
{
- NavigateTo::NavigateTo(const NavigateTo::Context& ctx) :
- Base(DefaultSkillDescription()), ctx(ctx)
+ NavigateTo::NavigateTo() : Base(DefaultSkillDescription())
{
}
+ void
+ NavigateTo::connect(const Services& srv)
+ {
+ srv_.emplace(srv);
+ }
+
::armarx::skills::Skill::InitResult
NavigateTo::init(const Base::SpecializedInitInput& in)
{
@@ -33,19 +37,19 @@ namespace armarx::navigation::skills
cfg.general(client::GeneralConfig{});
cfg.globalPlanner(armarx::navigation::global_planning::SPFAParams{});
cfg.trajectoryController(
- armarx::navigation::traj_ctrl::TrajectoryFollowingControllerParams{});
+ armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams{});
const std::string configId = DefaultSkillDescription().skillName;
// configure the `navigator` which provides a simplified and typed interface to the navigation server
- memorySubscriber.emplace(configId, ctx.memoryNameSystem);
+ memorySubscriber.emplace(configId, srv_->memoryNameSystem);
// register our config
ARMARX_INFO << "Registering config";
- ctx.iceNavigator.createConfig(cfg, configId);
+ srv_->iceNavigator.createConfig(cfg, configId);
navigator.emplace(client::Navigator::InjectedServices{
- .navigator = &ctx.iceNavigator, .subscriber = &memorySubscriber.value()});
+ .navigator = &srv_->iceNavigator, .subscriber = &memorySubscriber.value()});
return ::armarx::skills::Skill::InitResult{
.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
diff --git a/source/armarx/navigation/skills/NavigateTo.h b/source/armarx/navigation/skills/NavigateTo.h
index 085d138f6ca3137077cc20c513e594a2f48e666c..c0a39f601e981e197723e65ad9ef57ec6ad6fbe1 100644
--- a/source/armarx/navigation/skills/NavigateTo.h
+++ b/source/armarx/navigation/skills/NavigateTo.h
@@ -41,13 +41,15 @@ namespace armarx::navigation::skills
using Base = ::armarx::skills::SpecializedSkill<Params>;
- struct Context
+ struct Services
{
client::IceNavigator& iceNavigator;
armem::client::MemoryNameSystem& memoryNameSystem;
};
- NavigateTo(const Context& ctx);
+ NavigateTo();
+
+ void connect(const Services& srv);
private:
::armarx::skills::Skill::InitResult init(const Base::SpecializedInitInput& in) override;
@@ -70,7 +72,7 @@ namespace armarx::navigation::skills
protected:
private:
- const Context ctx;
+ std::optional<Services> srv_;
// will be initialized in "init()"
std::optional<client::MemorySubscriber> memorySubscriber;
diff --git a/source/armarx/navigation/skills/NavigateToLocation.cpp b/source/armarx/navigation/skills/NavigateToLocation.cpp
index d9d52ceb37bbe5f2c5b37b0aa398d4a9dea0f528..c488c5c05c205a503933f7a4356add8ff113aa52 100644
--- a/source/armarx/navigation/skills/NavigateToLocation.cpp
+++ b/source/armarx/navigation/skills/NavigateToLocation.cpp
@@ -5,26 +5,30 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include "armarx/navigation/client/services/MemorySubscriber.h"
-#include "armarx/navigation/global_planning/SPFA.h"
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/client/services/IceNavigator.h>
+#include <armarx/navigation/client/services/MemorySubscriber.h>
#include <armarx/navigation/client/services/SimpleEventHandler.h>
#include <armarx/navigation/client/types.h>
#include <armarx/navigation/global_planning/AStar.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/global_planning/SPFA.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
namespace armarx::navigation::skills
{
- NavigateToLocation::NavigateToLocation(const NavigateToLocation::Context& ctx) :
- Base(DefaultSkillDescription()), ctx(ctx)
+ NavigateToLocation::NavigateToLocation() : Base(DefaultSkillDescription())
{
}
+ void
+ NavigateToLocation::connect(const Services& srv)
+ {
+ srv_.emplace(srv);
+ }
+
::armarx::skills::Skill::InitResult
NavigateToLocation::init(const Base::SpecializedInitInput& in)
{
@@ -33,19 +37,23 @@ namespace armarx::navigation::skills
cfg.general(client::GeneralConfig{});
cfg.globalPlanner(armarx::navigation::global_planning::SPFAParams{});
cfg.trajectoryController(
- armarx::navigation::traj_ctrl::TrajectoryFollowingControllerParams{});
+ armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams{});
const std::string configId = DefaultSkillDescription().skillName;
// configure the `navigator` which provides a simplified and typed interface to the navigation server
- memorySubscriber.emplace(configId, ctx.memoryNameSystem);
+ ARMARX_TRACE;
+ memorySubscriber.reset();
+
+ ARMARX_TRACE;
+ memorySubscriber.emplace(configId, srv_->memoryNameSystem);
// register our config
ARMARX_INFO << "Registering config";
- ctx.iceNavigator.createConfig(cfg, configId);
+ srv_->iceNavigator.createConfig(cfg, configId);
navigator.emplace(client::Navigator::InjectedServices{
- .navigator = &ctx.iceNavigator, .subscriber = &memorySubscriber.value()});
+ .navigator = &srv_->iceNavigator, .subscriber = &memorySubscriber.value()});
return ::armarx::skills::Skill::InitResult{
.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
@@ -54,17 +62,17 @@ namespace armarx::navigation::skills
::armarx::skills::Skill::MainResult
NavigateToLocation::main(const Base::SpecializedMainInput& in)
{
- const std::string location = in.params.location;
+ const std::string& location = in.params.location;
ARMARX_INFO << "moving to target " << VAROUT(location);
// execute
ARMARX_INFO << "Sending navigation request";
- client::PathBuilder builder;
- builder.add(location, client::GlobalPlanningStrategy::Free);
+ // client::PathBuilder builder;
+ // builder.add(location, client::GlobalPlanningStrategy::Free);
- navigator->moveTo(builder, core::NavigationFrame::Absolute);
+ navigator->moveToLocation(location);
// Wait until goal is reached
ARMARX_INFO << "Waiting until goal is reached.";
diff --git a/source/armarx/navigation/skills/NavigateToLocation.h b/source/armarx/navigation/skills/NavigateToLocation.h
index e24d61972607ca7ca7b31fde9937ce13d1d80202..fcf5d532f28fe939448d1e7e7c5c93c9aa5f2b8d 100644
--- a/source/armarx/navigation/skills/NavigateToLocation.h
+++ b/source/armarx/navigation/skills/NavigateToLocation.h
@@ -42,14 +42,16 @@ namespace armarx::navigation::skills
using Base = ::armarx::skills::SpecializedSkill<Params>;
- struct Context
+ struct Services
{
client::IceNavigator& iceNavigator;
armem::client::MemoryNameSystem& memoryNameSystem;
};
- NavigateToLocation(const Context& ctx);
+ NavigateToLocation();
+
+ void connect(const Services& srv);
private:
::armarx::skills::Skill::InitResult init(const Base::SpecializedInitInput& in) override;
@@ -72,7 +74,7 @@ namespace armarx::navigation::skills
protected:
private:
- const Context ctx;
+ std::optional<Services> srv_;
// will be initialized in "init()"
std::optional<client::MemorySubscriber> memorySubscriber;
diff --git a/source/armarx/navigation/statecharts/NavigationCommands/development/TestRotate90Degrees.xml b/source/armarx/navigation/statecharts/NavigationCommands/development/TestRotate90Degrees.xml
index abe107295245cffa0852e7a6a9e40b0926742083..c1417dbd435d29d9a77090e8e2397545147d081c 100644
--- a/source/armarx/navigation/statecharts/NavigationCommands/development/TestRotate90Degrees.xml
+++ b/source/armarx/navigation/statecharts/NavigationCommands/development/TestRotate90Degrees.xml
@@ -4,9 +4,9 @@
<OutputParameters/>
<LocalParameters/>
<Substates>
- <EndState name="Failure" event="Failure" left="363.333" top="140" boundingSquareSize="99.6636"/>
+ <EndState name="Failure" event="Failure" left="363.167" top="140" boundingSquareSize="99.6636"/>
<LocalState name="RotateXDegrees" refuuid="70E5C859-AC39-4B85-9981-6D0615DEAB13" left="114.167" top="195.917" boundingSquareSize="99.6636"/>
- <EndState name="Success" event="Success" left="363.333" top="252.139" boundingSquareSize="99.6636"/>
+ <EndState name="Success" event="Success" left="363.167" top="252.139" boundingSquareSize="99.6636"/>
</Substates>
<Events>
<Event name="Failure">
diff --git a/source/armarx/navigation/statecharts/NavigationGroup/NavigateRelative.cpp b/source/armarx/navigation/statecharts/NavigationGroup/NavigateRelative.cpp
index c30e1aff2aff2a97ab221aa1fe6f562c6889fd4f..f0f6785c3cb9995c9704aa44e972dfd63a66a9da 100644
--- a/source/armarx/navigation/statecharts/NavigationGroup/NavigateRelative.cpp
+++ b/source/armarx/navigation/statecharts/NavigationGroup/NavigateRelative.cpp
@@ -26,7 +26,7 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include "armarx/navigation/global_planning/Point2Point.h"
+#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/PathBuilder.h>
@@ -34,8 +34,7 @@
#include <armarx/navigation/client/services/MemoryPolling.h>
#include <armarx/navigation/client/services/SimpleEventHandler.h>
#include <armarx/navigation/client/types.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
namespace armarx::navigation::statecharts::navigation_group
{
@@ -62,7 +61,7 @@ namespace armarx::navigation::statecharts::navigation_group
cfg.general(client::GeneralConfig{});
cfg.globalPlanner(armarx::navigation::global_planning::Point2PointParams{});
cfg.trajectoryController(
- armarx::navigation::traj_ctrl::TrajectoryFollowingControllerParams{});
+ armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams{});
// configure the `navigator` which provides a simplified and typed interface to the navigation server
client::IceNavigator iceNavigator(getNavigator());
diff --git a/source/armarx/navigation/statecharts/NavigationGroup/NavigateTo.cpp b/source/armarx/navigation/statecharts/NavigationGroup/NavigateTo.cpp
index af7b10d4190311b2875bfe10f70fa7cf012c52c9..3def882cb54f018827a387ab4bc17ad8aea3a9a7 100644
--- a/source/armarx/navigation/statecharts/NavigationGroup/NavigateTo.cpp
+++ b/source/armarx/navigation/statecharts/NavigationGroup/NavigateTo.cpp
@@ -30,7 +30,8 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include "armarx/navigation/client/services/MemoryPolling.h"
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
+#include <armarx/navigation/client/services/MemoryPolling.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/PathBuilder.h>
@@ -38,8 +39,6 @@
#include <armarx/navigation/client/services/SimpleEventHandler.h>
#include <armarx/navigation/client/types.h>
#include <armarx/navigation/global_planning/AStar.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
//#include <ArmarXCore/core/time/TimeUtil.h>
@@ -77,7 +76,7 @@ namespace armarx::navigation::statecharts::navigation_group
cfg.general(client::GeneralConfig{});
cfg.globalPlanner(armarx::navigation::global_planning::AStarParams{});
cfg.trajectoryController(
- armarx::navigation::traj_ctrl::TrajectoryFollowingControllerParams{});
+ traj_ctrl::local::TrajectoryFollowingControllerParams{});
// configure the `navigator` which provides a simplified and typed interface to the navigation server
client::IceNavigator iceNavigator(getNavigator());
diff --git a/source/armarx/navigation/statecharts/NavigationGroup/NavigateTo.xml b/source/armarx/navigation/statecharts/NavigationGroup/NavigateTo.xml
index 2cd2476be8750eac523cb21ed336702a19b365a0..ca00d381094a24bc769aff3269dfa430d2afc09a 100644
--- a/source/armarx/navigation/statecharts/NavigationGroup/NavigateTo.xml
+++ b/source/armarx/navigation/statecharts/NavigationGroup/NavigateTo.xml
@@ -2,7 +2,7 @@
<State version="1.2" name="NavigateTo" uuid="BCF0FD77-35EE-4916-ADC1-4C7D05B502EB" width="800" height="600" type="Normal State">
<InputParameters>
<Parameter name="TargetPosition" type="::armarx::Vector3Base" docType="Vector3" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":1200,"y":2000,"z":0}}}' docValue="1200\n2000\n0"/>
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":2000,"y":2000,"z":0}}}' docValue="2000\n2000\n0"/>
</Parameter>
</InputParameters>
<OutputParameters/>
diff --git a/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp b/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp
index fa2ff12d4d0e454120e2ea64951f4134ce085912..ca748edcab7d9ebc8234b81d5c81c4a6ee36ac8d 100644
--- a/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp
+++ b/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp
@@ -28,7 +28,7 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include "armarx/navigation/client/services/MemoryPolling.h"
+#include <armarx/navigation/client/services/MemoryPolling.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/PathBuilder.h>
@@ -36,8 +36,8 @@
#include <armarx/navigation/client/services/SimpleEventHandler.h>
#include <armarx/navigation/client/types.h>
#include <armarx/navigation/global_planning/AStar.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
//#include <ArmarXCore/core/time/TimeUtil.h>
//#include <ArmarXCore/observers/variant/DatafieldRef.h>
@@ -67,7 +67,7 @@ namespace armarx::navigation::statecharts::navigation_group
cfg.general(client::GeneralConfig{});
cfg.globalPlanner(armarx::navigation::global_planning::AStarParams{});
cfg.trajectoryController(
- armarx::navigation::traj_ctrl::TrajectoryFollowingControllerParams{});
+ armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams{});
// configure the `navigator` which provides a simplified and typed interface to the navigation server
client::IceNavigator iceNavigator(getNavigator());
@@ -83,13 +83,11 @@ namespace armarx::navigation::statecharts::navigation_group
client::Navigator navigator{client::Navigator::InjectedServices{
.navigator = &iceNavigator, .subscriber = &memoryEventPolling}};
- // assemble the path, which might consist of waypoints and a goal (the goal is just the last `waypoint`)
- client::PathBuilder builder;
- builder.add(in.getlocation(), client::GlobalPlanningStrategy::Point2Point);
+ ARMARX_CHECK_NOT_EMPTY(in.getlocation());
// execute
ARMARX_INFO << "Sending navigation request";
- navigator.moveTo(builder, core::NavigationFrame::Absolute);
+ navigator.moveToLocation(in.getlocation());
// Wait until goal is reached
ARMARX_INFO << "Waiting until goal is reached.";
diff --git a/source/armarx/navigation/trajectory_control/CMakeLists.txt b/source/armarx/navigation/trajectory_control/CMakeLists.txt
index 07c8a589b2d160fc1a60708fe281b13e5983f1e7..8057dd3aabc281f435ee91e495cdc82a77e68fa3 100644
--- a/source/armarx/navigation/trajectory_control/CMakeLists.txt
+++ b/source/armarx/navigation/trajectory_control/CMakeLists.txt
@@ -1,34 +1,66 @@
-armarx_add_aron_library(trajectory_control_aron
+armarx_add_aron_library(trajectory_control_local_aron
ARON_FILES
- aron/TrajectoryControllerParams.xml
- aron/TrajectoryFollowingControllerParams.xml
- aron/WaypointControllerParams.xml
+ local/aron/TrajectoryControllerParams.xml
+ local/aron/TrajectoryFollowingControllerParams.xml
)
-armarx_add_library(trajectory_control
+armarx_add_aron_library(trajectory_control_global_aron
+ ARON_FILES
+ global/aron/TrajectoryControllerParams.xml
+ global/aron/TrajectoryFollowingControllerParams.xml
+ global/aron/WaypointControllerParams.xml
+)
+
+
+armarx_add_library(trajectory_control_local
SOURCES
- ./TrajectoryController.cpp
- ./TrajectoryFollowingController.cpp
- ./WaypointController.cpp
- ./aron_conversions.cpp
+ local/TrajectoryFollowingController.cpp
+ local/aron_conversions.cpp
HEADERS
- ./TrajectoryController.h
- ./TrajectoryFollowingController.h
- ./WaypointController.h
- ./aron_conversions.h
- ./core.h
+ local/TrajectoryController.h
+ local/TrajectoryFollowingController.h
+ local/aron_conversions.h
+ local/core.h
DEPENDENCIES
ArmarXCoreInterfaces
ArmarXCore
RobotAPICore
armarx_navigation::core
- armarx_navigation::trajectory_control_aron
+ armarx_navigation::trajectory_control_local_aron
+ OBJECT
)
-armarx_add_test(trajectory_control_test
- TEST_FILES
- test/trajectory_controlTest.cpp
+armarx_add_library(trajectory_control_global
+ SOURCES
+ global/aron_conversions.cpp
+ global/TrajectoryFollowingController.cpp
+ global/WaypointController.cpp
+ HEADERS
+ global/TrajectoryController.h
+ global/aron_conversions.h
+ global/TrajectoryFollowingController.h
+ global/WaypointController.h
+ global/core.h
DEPENDENCIES
+ ArmarXCoreInterfaces
ArmarXCore
- armarx_navigation::trajectory_control
+ RobotAPICore
+ armarx_navigation::core
+ armarx_navigation::trajectory_control_global_aron
+ OBJECT
+)
+
+armarx_add_library(trajectory_control
+ DEPENDENCIES
+ armarx_navigation::trajectory_control_local
+ armarx_navigation::trajectory_control_global
)
+
+
+# armarx_add_test(trajectory_control_test
+# TEST_FILES
+# test/trajectory_controlTest.cpp
+# DEPENDENCIES
+# ArmarXCore
+# armarx_navigation::trajectory_control
+# )
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
deleted file mode 100644
index be79e84e239d3269a3fe7d69c66ed9cb6364069d..0000000000000000000000000000000000000000
--- a/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
+++ /dev/null
@@ -1,9 +0,0 @@
-#include "TrajectoryController.h"
-
-namespace armarx::navigation::traj_ctrl
-{
- // TrajectoryController::TrajectoryController(const core::Scene& context) : context(context)
- // {
- // }
-
-} // namespace armarx::navigation::traj_ctrl
diff --git a/source/armarx/navigation/trajectory_control/global/TrajectoryController.h b/source/armarx/navigation/trajectory_control/global/TrajectoryController.h
new file mode 100644
index 0000000000000000000000000000000000000000..ddd843425f58a694fee33878d39482d767a84d4f
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/global/TrajectoryController.h
@@ -0,0 +1,77 @@
+/**
+ * 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 )
+ * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <cmath>
+#include <limits>
+#include <memory>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include <armarx/navigation/core/DynamicScene.h>
+#include <armarx/navigation/core/StaticScene.h>
+#include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/trajectory_control/global/core.h>
+
+namespace armarx::navigation::traj_ctrl
+{
+ namespace global
+ {
+
+ struct TrajectoryControllerResult
+ {
+ core::Twist twist;
+ core::GlobalTrajectoryPoint dropPoint;
+ };
+
+ struct TrajectoryControllerParams
+ {
+ core::TwistLimits limits{
+ .linear = 500.F, // [mm/s]
+ .angular = 2.F * M_PIf32 / 10.F // [rad/s]
+ };
+
+ virtual ~TrajectoryControllerParams() = default;
+
+ virtual Algorithms algorithm() const = 0;
+ virtual aron::data::DictPtr toAron() const = 0;
+ };
+
+ class TrajectoryController
+ {
+ public:
+ TrajectoryController() = default;
+ virtual ~TrajectoryController() = default;
+
+ virtual TrajectoryControllerResult control(const core::GlobalTrajectory& goal) = 0;
+ };
+ using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
+
+ } // namespace global
+
+
+
+} // namespace armarx::navigation::traj_ctrl
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.cpp
similarity index 92%
rename from source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
rename to source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.cpp
index b7fcdb2df4bdb2278050a16bceb3c425b58dfe81..264b6df47e9faf16575a5a90b452603e0787b089 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.cpp
@@ -11,15 +11,15 @@
#include <RobotAPI/libraries/core/MultiDimPIDController.h>
-#include "armarx/navigation/core/basic_types.h"
+#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/types.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h>
-#include <armarx/navigation/trajectory_control/aron_conversions.h>
-#include <armarx/navigation/trajectory_control/core.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/global/core.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
// TrajectoryFollowingControllerParams
@@ -35,7 +35,7 @@ namespace armarx::navigation::traj_ctrl
arondto::TrajectoryFollowingControllerParams dto;
TrajectoryFollowingControllerParams bo;
- armarx::navigation::traj_ctrl::toAron(dto, bo);
+ armarx::navigation::traj_ctrl::global::toAron(dto, bo);
return dto.toAron();
}
@@ -47,7 +47,7 @@ namespace armarx::navigation::traj_ctrl
dto.fromAron(dict);
TrajectoryFollowingControllerParams bo;
- armarx::navigation::traj_ctrl::fromAron(dto, bo);
+ armarx::navigation::traj_ctrl::global::fromAron(dto, bo);
return bo;
}
@@ -127,7 +127,7 @@ namespace armarx::navigation::traj_ctrl
}
TrajectoryControllerResult
- TrajectoryFollowingController::control(const core::Trajectory& trajectory)
+ TrajectoryFollowingController::control(const core::GlobalTrajectory& trajectory)
{
ARMARX_CHECK_NOT_NULL(robot) << "Robot not available";
@@ -217,4 +217,4 @@ namespace armarx::navigation::traj_ctrl
.dropPoint = projectedPose.projection};
}
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::global
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h b/source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h
similarity index 86%
rename from source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
rename to source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h
index ffb81732fa4f599c341a3e1528bb791ab4a4395b..a8d416f60e9702b8f06adb1f0aa5eb57b514e6a2 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
+++ b/source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h
@@ -24,10 +24,10 @@
#include <RobotAPI/libraries/core/MultiDimPIDController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.aron.generated.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
// using arondto::TrajectoryFollowingControllerParams;
@@ -51,7 +51,7 @@ namespace armarx::navigation::traj_ctrl
TrajectoryFollowingController(const VirtualRobot::RobotPtr& robot, const Params& params);
~TrajectoryFollowingController() override = default;
- TrajectoryControllerResult control(const core::Trajectory& trajectory) override;
+ TrajectoryControllerResult control(const core::GlobalTrajectory& trajectory) override;
// protected:
core::Twist applyTwistLimits(core::Twist twist);
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.cpp b/source/armarx/navigation/trajectory_control/global/WaypointController.cpp
similarity index 61%
rename from source/armarx/navigation/trajectory_control/WaypointController.cpp
rename to source/armarx/navigation/trajectory_control/global/WaypointController.cpp
index 527bd4f3a485f233f77db40ff9f55e5d54319117..ae8f08842df740579409dbcd971c5414cb047e3e 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.cpp
+++ b/source/armarx/navigation/trajectory_control/global/WaypointController.cpp
@@ -1,11 +1,11 @@
#include "WaypointController.h"
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/aron/WaypointControllerParams.aron.generated.h>
-#include <armarx/navigation/trajectory_control/aron_conversions.h>
-#include <armarx/navigation/trajectory_control/core.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/aron/WaypointControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/global/core.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
// WaypointControllerParams
@@ -21,7 +21,7 @@ namespace armarx::navigation::traj_ctrl
arondto::WaypointControllerParams dto;
WaypointControllerParams bo;
- armarx::navigation::traj_ctrl::toAron(dto, bo);
+ armarx::navigation::traj_ctrl::global::toAron(dto, bo);
return dto.toAron();
}
@@ -33,7 +33,7 @@ namespace armarx::navigation::traj_ctrl
dto.fromAron(dict);
WaypointControllerParams bo;
- armarx::navigation::traj_ctrl::fromAron(dto, bo);
+ armarx::navigation::traj_ctrl::global::fromAron(dto, bo);
return bo;
}
@@ -46,7 +46,7 @@ namespace armarx::navigation::traj_ctrl
}
TrajectoryControllerResult
- WaypointController::control(const core::Trajectory& goal)
+ WaypointController::control(const core::GlobalTrajectory& goal)
{
return {}; // TODO implement
}
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.h b/source/armarx/navigation/trajectory_control/global/WaypointController.h
similarity index 85%
rename from source/armarx/navigation/trajectory_control/WaypointController.h
rename to source/armarx/navigation/trajectory_control/global/WaypointController.h
index ec5d4372840eadb710bdd8708fcd829cedc4038e..c87fe9ad9a980c50f5741b8a2daef3a0877c1f82 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.h
+++ b/source/armarx/navigation/trajectory_control/global/WaypointController.h
@@ -22,9 +22,9 @@
#pragma once
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
struct WaypointControllerParams : public TrajectoryControllerParams
@@ -42,7 +42,7 @@ namespace armarx::navigation::traj_ctrl
WaypointController(const Params& params);
~WaypointController() override = default;
- TrajectoryControllerResult control(const core::Trajectory& goal) override;
+ TrajectoryControllerResult control(const core::GlobalTrajectory& goal) override;
protected:
private:
@@ -50,4 +50,4 @@ namespace armarx::navigation::traj_ctrl
};
using WaypointControllerPtr = std::shared_ptr<WaypointController>;
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::global
diff --git a/source/armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.xml b/source/armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..d6666423d2689981b8d4f252289600c89d660507
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ <Include include="armarx/navigation/core/aron/TwistLimits.xml" />
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::navigation::traj_ctrl::global::arondto::TrajectoryControllerParams'>
+ <!-- <ObjectChild key='limits'>
+ <armarx::navigation::core::arondto::TwistLimits />
+ </ObjectChild> -->
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.xml b/source/armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.xml
similarity index 80%
rename from source/armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.xml
rename to source/armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.xml
index 860dcfe1aa63c636d87768fcc18f1f62621f5ecd..8c9c8cea2502aac7345727dcd0d88eaa25c334f7 100644
--- a/source/armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.xml
+++ b/source/armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.xml
@@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
- <Include include="armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.xml" />
+ <Include include="armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.xml" />
<Include include="armarx/navigation/core/aron/PIDParams.xml" />
<Include include="armarx/navigation/core/aron/TwistLimits.xml" />
</AronIncludes>
@@ -9,7 +9,7 @@
<GenerateTypes>
<!-- <Object name='armarx::navigation::traj_ctrl::arondto::TrajectoryFollowingControllerParams' extends="armarx::navigation::traj_ctrl::arondto::TrajectoryControllerParams"> -->
- <Object name='armarx::navigation::traj_ctrl::arondto::TrajectoryFollowingControllerParams'>
+ <Object name='armarx::navigation::traj_ctrl::global::arondto::TrajectoryFollowingControllerParams'>
<ObjectChild key='pidPos'>
<armarx::navigation::core::arondto::PIDParams />
</ObjectChild>
diff --git a/source/armarx/navigation/trajectory_control/aron/WaypointControllerParams.xml b/source/armarx/navigation/trajectory_control/global/aron/WaypointControllerParams.xml
similarity index 72%
rename from source/armarx/navigation/trajectory_control/aron/WaypointControllerParams.xml
rename to source/armarx/navigation/trajectory_control/global/aron/WaypointControllerParams.xml
index 6a63d73467f6fdf6a447213277b661afbf0cc31e..eb54f90cb56b9f9fb594036b86a69d5eba501df7 100644
--- a/source/armarx/navigation/trajectory_control/aron/WaypointControllerParams.xml
+++ b/source/armarx/navigation/trajectory_control/global/aron/WaypointControllerParams.xml
@@ -1,13 +1,13 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
- <Include include="armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.xml" />
+ <Include include="armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.xml" />
</AronIncludes>
<GenerateTypes>
<!-- <Object name='armarx::navigation::traj_ctrl::arondto::WaypointControllerParams' extends="armarx::navigation::traj_ctrl::arondto::TrajectoryControllerParams"> -->
- <Object name='armarx::navigation::traj_ctrl::arondto::WaypointControllerParams'>
+ <Object name='armarx::navigation::traj_ctrl::global::arondto::WaypointControllerParams'>
<ObjectChild key='includeStartPose'>
<bool />
</ObjectChild>
diff --git a/source/armarx/navigation/trajectory_control/aron_conversions.cpp b/source/armarx/navigation/trajectory_control/global/aron_conversions.cpp
similarity index 74%
rename from source/armarx/navigation/trajectory_control/aron_conversions.cpp
rename to source/armarx/navigation/trajectory_control/global/aron_conversions.cpp
index 2cc2d0b42889e5b9323c600f6b0e734b2874f44f..d32a613238802200dc0bf59938a4887a86fc2c30 100644
--- a/source/armarx/navigation/trajectory_control/aron_conversions.cpp
+++ b/source/armarx/navigation/trajectory_control/global/aron_conversions.cpp
@@ -5,15 +5,15 @@
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
#include <armarx/navigation/core/aron_conversions.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
-#include <armarx/navigation/trajectory_control/WaypointController.h>
-#include <armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.aron.generated.h>
-#include <armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h>
-#include <armarx/navigation/trajectory_control/aron/WaypointControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/global/WaypointController.h>
+#include <armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/aron/WaypointControllerParams.aron.generated.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
void
@@ -67,4 +67,4 @@ namespace armarx::navigation::traj_ctrl
}
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::global
diff --git a/source/armarx/navigation/trajectory_control/aron_conversions.h b/source/armarx/navigation/trajectory_control/global/aron_conversions.h
similarity index 93%
rename from source/armarx/navigation/trajectory_control/aron_conversions.h
rename to source/armarx/navigation/trajectory_control/global/aron_conversions.h
index f8496c98a49cf147968a3e02f19a03cd271f6e37..3f0dbd61544175a19747be2ee9a9b8920dbb4592 100644
--- a/source/armarx/navigation/trajectory_control/aron_conversions.h
+++ b/source/armarx/navigation/trajectory_control/global/aron_conversions.h
@@ -22,9 +22,8 @@
#pragma once
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
- struct PIDParams;
struct TrajectoryControllerParams;
struct TrajectoryFollowingControllerParams;
@@ -32,7 +31,6 @@ namespace armarx::navigation::traj_ctrl
namespace arondto
{
- class PIDParams;
class TrajectoryControllerParams;
class TrajectoryFollowingControllerParams;
@@ -51,4 +49,4 @@ namespace armarx::navigation::traj_ctrl
void toAron(arondto::WaypointControllerParams& dto, const WaypointControllerParams& bo);
void fromAron(const arondto::WaypointControllerParams& dto, WaypointControllerParams& bo);
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::global
diff --git a/source/armarx/navigation/trajectory_control/core.h b/source/armarx/navigation/trajectory_control/global/core.h
similarity index 89%
rename from source/armarx/navigation/trajectory_control/core.h
rename to source/armarx/navigation/trajectory_control/global/core.h
index 0cb073307f166c091a5f7477ba2ea829397d40f3..2c2d4674a6e9abf26201b67fc81d2146d24631cc 100644
--- a/source/armarx/navigation/trajectory_control/core.h
+++ b/source/armarx/navigation/trajectory_control/global/core.h
@@ -24,17 +24,17 @@
#include <SimoxUtility/meta/enum/EnumNames.hpp>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
enum class Algorithms
{
WaypointController,
- TrajectoryFollowingController
+ TrajectoryFollowingController,
};
const inline simox::meta::EnumNames<Algorithms> AlgorithmNames{
{Algorithms::WaypointController, "WaypointController"},
{Algorithms::TrajectoryFollowingController, "TrajectoryFollowingController"}};
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::global
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.h b/source/armarx/navigation/trajectory_control/local/TrajectoryController.h
similarity index 90%
rename from source/armarx/navigation/trajectory_control/TrajectoryController.h
rename to source/armarx/navigation/trajectory_control/local/TrajectoryController.h
index 72d5927d31f6c8b0885997a25c1aee3ce0ccb853..4a5c3bbe3ebba65e367884d2e5f8098b0a6b6681 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryController.h
+++ b/source/armarx/navigation/trajectory_control/local/TrajectoryController.h
@@ -36,13 +36,12 @@
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/types.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::local
{
struct TrajectoryControllerResult
{
core::Twist twist;
- core::TrajectoryPoint dropPoint;
};
struct TrajectoryControllerParams
@@ -64,9 +63,8 @@ namespace armarx::navigation::traj_ctrl
TrajectoryController() = default;
virtual ~TrajectoryController() = default;
- virtual TrajectoryControllerResult control(const core::Trajectory& goal) = 0;
-
+ virtual TrajectoryControllerResult control(const core::LocalTrajectory& goal) = 0;
};
using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::local
diff --git a/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e95af6459d3c1ffa9b831dde3f0db7573730051c
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp
@@ -0,0 +1,186 @@
+#include "TrajectoryFollowingController.h"
+
+#include <algorithm>
+
+#include <SimoxUtility/math/convert/mat4f_to_pos.h>
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
+#include <VirtualRobot/Robot.h>
+
+#include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/DateTime.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
+
+#include <RobotAPI/libraries/core/MultiDimPIDController.h>
+
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/local/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/local/core.h>
+
+namespace armarx::navigation::traj_ctrl::local
+{
+ // TrajectoryFollowingControllerParams
+
+ Algorithms
+ TrajectoryFollowingControllerParams::algorithm() const
+ {
+ return Algorithms::TrajectoryFollowingController;
+ }
+
+ aron::data::DictPtr
+ TrajectoryFollowingControllerParams::toAron() const
+ {
+ arondto::TrajectoryFollowingControllerParams dto;
+
+ TrajectoryFollowingControllerParams bo;
+ armarx::navigation::traj_ctrl::local::toAron(dto, bo);
+
+ return dto.toAron();
+ }
+
+ TrajectoryFollowingControllerParams
+ TrajectoryFollowingControllerParams::FromAron(const aron::data::DictPtr& dict)
+ {
+ arondto::TrajectoryFollowingControllerParams dto;
+ dto.fromAron(dict);
+
+ TrajectoryFollowingControllerParams bo;
+ armarx::navigation::traj_ctrl::local::fromAron(dto, bo);
+
+ return bo;
+ }
+
+ // TrajectoryFollowingController
+
+ TrajectoryFollowingController::TrajectoryFollowingController(
+ const VirtualRobot::RobotPtr& robot,
+ const Params& params) :
+ robot(robot),
+ params(params),
+ pidPos(params.pidPos.Kp,
+ params.pidPos.Ki,
+ params.pidPos.Kd,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ false,
+ std::vector<bool>{false, false, false}),
+ pidPosTarget(params.pidPos.Kp / 2,
+ params.pidPos.Ki,
+ params.pidPos.Kd,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ false,
+ std::vector<bool>{false, false, false}),
+ pidOri(params.pidOri.Kp,
+ params.pidOri.Ki,
+ params.pidOri.Kd,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ false,
+ std::vector<bool>{true, true, true}),
+ pidOriTarget(params.pidOri.Kp,
+ params.pidOri.Ki,
+ params.pidOri.Kd,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ false,
+ std::vector<bool>{true, true, true})
+ {
+ ARMARX_IMPORTANT << "Trajectory following controller params: "
+ << VAROUT(params.limits.linear) << ", " << VAROUT(params.limits.angular);
+ }
+
+ core::Twist
+ TrajectoryFollowingController::applyTwistLimits(core::Twist twist)
+ {
+ const core::Twist limits{.linear = Eigen::Vector3f::Ones() * params.limits.linear,
+ .angular = Eigen::Vector3f::Ones() * params.limits.angular};
+
+ // for all entries, scale should be less than 1
+ const auto scalePos = twist.linear.cwiseAbs().cwiseQuotient(limits.linear.cwiseAbs());
+ const auto scaleOri = twist.angular.cwiseAbs().cwiseQuotient(limits.angular.cwiseAbs());
+
+ const float scaleMax = std::max(scalePos.maxCoeff(), scaleOri.maxCoeff());
+
+ if (scaleMax < 1.0F) // both linear and angular velocity in bounds?
+ {
+ return twist;
+ }
+
+ // scale such that no limit is violated
+ twist.linear /= scaleMax;
+ twist.angular /= scaleMax;
+
+ constexpr float eps = 0.001;
+
+ // pedantic checks
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular + eps);
+
+ return twist;
+ }
+
+ TrajectoryControllerResult
+ TrajectoryFollowingController::control(const core::LocalTrajectory& trajectory)
+ {
+ ARMARX_CHECK_NOT_NULL(robot) << "Robot not available";
+
+ if (trajectory.points().empty())
+ {
+ ARMARX_INFO << "Trajectory is empty.";
+ return TrajectoryControllerResult{.twist = core::Twist::Zero()};
+ }
+
+ const core::Pose currentPose(robot->getGlobalPose());
+
+ // TOOD maybe set via arg?
+ const DateTime timestamp = Clock::Now();
+
+ const core::Evaluation target = trajectory.evaluate(timestamp);
+
+ using simox::math::mat4f_to_pos;
+ using simox::math::mat4f_to_rpy;
+
+ pidPos.update(mat4f_to_pos(currentPose.matrix()), mat4f_to_pos(target.pose.matrix()));
+ pidOri.update(mat4f_to_rpy(currentPose.matrix()), mat4f_to_rpy(target.pose.matrix()));
+
+
+ const core::Twist& twistFeedForward = target.feedforwardTwist;
+
+ const core::Twist twistError{.linear = pidPos.getControlValue(),
+ .angular = pidOri.getControlValue()};
+
+ const core::Twist twist
+ {
+ .linear = twistFeedForward.linear + twistError.linear,
+ .angular = twistFeedForward.angular + twistError.angular
+ };
+
+ const auto twistLimited = applyTwistLimits(twist);
+ ARMARX_VERBOSE << deactivateSpam(1) << "Twist limited " << twistLimited.linear.transpose();
+ ARMARX_VERBOSE << deactivateSpam(1) << "Twist angular " << twistLimited.angular.transpose();
+
+ // convert to the robot's base frame
+ const core::Pose global_T_robot(robot->getGlobalPose());
+
+ const auto& twistGlobal = twistLimited;
+
+ core::Twist twistLocal;
+ twistLocal.linear = global_T_robot.linear().inverse() * twistGlobal.linear;
+ // TODO if not in 2D, then this must be changed!
+ twistLocal.angular = twistGlobal.angular;
+
+ return TrajectoryControllerResult{.twist = twistLocal};
+ }
+
+} // namespace armarx::navigation::traj_ctrl::local
diff --git a/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h b/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h
new file mode 100644
index 0000000000000000000000000000000000000000..6484ebfecaf7bbeebecd3abcdcbc50b04df6b2f0
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h
@@ -0,0 +1,71 @@
+/**
+ * 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 )
+ * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/core/MultiDimPIDController.h>
+
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.aron.generated.h>
+
+namespace armarx::navigation::traj_ctrl::local
+{
+ // using arondto::LocalTrajectoryFollowingControllerParams;
+
+ struct TrajectoryFollowingControllerParams : public TrajectoryControllerParams
+ {
+
+ core::PIDParams pidPos;
+ core::PIDParams pidOri;
+
+ Algorithms algorithm() const override;
+ aron::data::DictPtr toAron() const override;
+
+ static TrajectoryFollowingControllerParams FromAron(const aron::data::DictPtr& dict);
+ };
+
+ class TrajectoryFollowingController : virtual public TrajectoryController
+ {
+ public:
+ using Params = TrajectoryFollowingControllerParams;
+
+ TrajectoryFollowingController(const VirtualRobot::RobotPtr& robot, const Params& params);
+ ~TrajectoryFollowingController() override = default;
+
+ TrajectoryControllerResult control(const core::LocalTrajectory& trajectory) override;
+
+ // protected:
+ core::Twist applyTwistLimits(core::Twist twist);
+
+ private:
+ const VirtualRobot::RobotPtr robot;
+ const Params params;
+
+
+ MultiDimPIDControllerTemplate<3> pidPos;
+ MultiDimPIDControllerTemplate<3> pidPosTarget;
+ MultiDimPIDControllerTemplate<3> pidOri;
+ MultiDimPIDControllerTemplate<3> pidOriTarget;
+ };
+ using LocalTrajectoryFollowingControllerPtr = std::shared_ptr<TrajectoryFollowingController>;
+
+} // namespace armarx::navigation::traj_ctrl::local
diff --git a/source/armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.xml b/source/armarx/navigation/trajectory_control/local/aron/TrajectoryControllerParams.xml
similarity index 80%
rename from source/armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.xml
rename to source/armarx/navigation/trajectory_control/local/aron/TrajectoryControllerParams.xml
index 44429d94deb4fdb36468b91c2063268b5ad73e07..b83855953cc0d5285dd6ba01f7465c2f3b44fce4 100644
--- a/source/armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.xml
+++ b/source/armarx/navigation/trajectory_control/local/aron/TrajectoryControllerParams.xml
@@ -6,7 +6,7 @@
<GenerateTypes>
- <Object name='armarx::navigation::traj_ctrl::arondto::TrajectoryControllerParams'>
+ <Object name='armarx::navigation::traj_ctrl::local::arondto::TrajectoryControllerParams'>
<!-- <ObjectChild key='limits'>
<armarx::navigation::core::arondto::TwistLimits />
</ObjectChild> -->
diff --git a/source/armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.xml b/source/armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..d4bca2ab58629192d0170710fdd6950e33232b24
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ <Include include="armarx/navigation/trajectory_control/local/aron/TrajectoryControllerParams.xml" />
+ <Include include="armarx/navigation/core/aron/PIDParams.xml" />
+ <Include include="armarx/navigation/core/aron/TwistLimits.xml" />
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <!-- <Object name='armarx::navigation::traj_ctrl::arondto::LocalTrajectoryFollowingControllerParams' extends="armarx::navigation::traj_ctrl::arondto::TrajectoryControllerParams"> -->
+ <Object name='armarx::navigation::traj_ctrl::local::arondto::TrajectoryFollowingControllerParams'>
+ <ObjectChild key='pidPos'>
+ <armarx::navigation::core::arondto::PIDParams />
+ </ObjectChild>
+ <ObjectChild key='pidOri'>
+ <armarx::navigation::core::arondto::PIDParams />
+ </ObjectChild>
+ <ObjectChild key='limits'>
+ <armarx::navigation::core::arondto::TwistLimits />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/trajectory_control/local/aron_conversions.cpp b/source/armarx/navigation/trajectory_control/local/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8cfc96a7750da90a7be025e6b3c9999846e814ef
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/aron_conversions.cpp
@@ -0,0 +1,35 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+
+#include <armarx/navigation/core/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
+
+
+namespace armarx::navigation::traj_ctrl::local
+{
+
+
+ void
+ toAron(arondto::TrajectoryFollowingControllerParams& dto,
+ const TrajectoryFollowingControllerParams& bo)
+ {
+ core::toAron(dto.pidOri, bo.pidOri);
+ core::toAron(dto.pidPos, bo.pidPos);
+ core::toAron(dto.limits, bo.limits);
+ }
+
+ void
+ fromAron(const arondto::TrajectoryFollowingControllerParams& dto,
+ TrajectoryFollowingControllerParams& bo)
+ {
+ core::fromAron(dto.pidOri, bo.pidOri);
+ core::fromAron(dto.pidPos, bo.pidPos);
+ core::fromAron(dto.limits, bo.limits);
+ }
+
+
+} // namespace armarx::navigation::traj_ctrl::local
diff --git a/source/armarx/navigation/trajectory_control/local/aron_conversions.h b/source/armarx/navigation/trajectory_control/local/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..91b8f9bb8303a9cb531abd281a774632907e9477
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/aron_conversions.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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+namespace armarx::navigation::traj_ctrl::local
+{
+ struct TrajectoryControllerParams;
+ struct TrajectoryFollowingControllerParams;
+
+ namespace arondto
+ {
+ class TrajectoryControllerParams;
+ class TrajectoryFollowingControllerParams;
+ } // namespace arondto
+
+
+ void toAron(arondto::TrajectoryControllerParams& dto, const TrajectoryControllerParams& bo);
+ void fromAron(const arondto::TrajectoryControllerParams& dto, TrajectoryControllerParams& bo);
+
+ void toAron(arondto::TrajectoryFollowingControllerParams& dto,
+ const TrajectoryFollowingControllerParams& bo);
+ void fromAron(const arondto::TrajectoryFollowingControllerParams& dto,
+ TrajectoryFollowingControllerParams& bo);
+
+} // namespace armarx::navigation::traj_ctrl::local
diff --git a/source/armarx/navigation/trajectory_control/local/core.h b/source/armarx/navigation/trajectory_control/local/core.h
new file mode 100644
index 0000000000000000000000000000000000000000..3a3aa6768a752bc4b14f721dd2b148ca134ff84d
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/core.h
@@ -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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+namespace armarx::navigation::traj_ctrl::local
+{
+ enum class Algorithms
+ {
+ TrajectoryFollowingController
+ };
+
+ const inline simox::meta::EnumNames<Algorithms> AlgorithmNames{
+ {Algorithms::TrajectoryFollowingController, "TrajectoryFollowingController"}};
+} // namespace armarx::navigation::traj_ctrl::local
diff --git a/source/armarx/navigation/util/Visualization.cpp b/source/armarx/navigation/util/Visualization.cpp
index 31ffe3ec6dc1d22182164a08900411ec3949996b..fb91bf23d78b48413287d80090aec3a6da07a908 100644
--- a/source/armarx/navigation/util/Visualization.cpp
+++ b/source/armarx/navigation/util/Visualization.cpp
@@ -23,7 +23,7 @@ namespace armarx::navigation::util
}
void
- Visualization::visualize(const core::Trajectory& trajectory)
+ Visualization::visualize(const core::GlobalTrajectory& trajectory)
{
// TODO(fabian.reister): more finegrained locking
std::lock_guard g{paramsMutex};
@@ -60,7 +60,7 @@ namespace armarx::navigation::util
void
- Visualization::visualizeTrajectory(const core::Trajectory& trajectory)
+ Visualization::visualizeTrajectory(const core::GlobalTrajectory& trajectory)
{
if (!params.enableTrajectoryVisualization)
{
diff --git a/source/armarx/navigation/util/Visualization.h b/source/armarx/navigation/util/Visualization.h
index 10db349a494d7baca4b72fa22d9ee51ca8797f2a..6f249d7eb9851b17f30fff3fa73c1c3afb3eb074 100644
--- a/source/armarx/navigation/util/Visualization.h
+++ b/source/armarx/navigation/util/Visualization.h
@@ -44,7 +44,7 @@ namespace armarx::navigation::util
void failed();
- void visualize(const core::Trajectory& trajectory);
+ void visualize(const core::GlobalTrajectory& trajectory);
void
updateParams(const Params& params)
@@ -54,7 +54,7 @@ namespace armarx::navigation::util
}
private:
- void visualizeTrajectory(const core::Trajectory& trajectory);
+ void visualizeTrajectory(const core::GlobalTrajectory& trajectory);
void asyncHideDelayed();
diff --git a/test b/test
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391