diff --git a/scenarios/NavigationSimulation/NavigationSimulation.scx b/scenarios/NavigationSimulation/NavigationSimulation.scx
index 7c8bf98a55547d65a724b3cc547bfb7195e0cde4..45bbd71e2f69db66f5741dbc97e2d0dada0be340 100644
--- a/scenarios/NavigationSimulation/NavigationSimulation.scx
+++ b/scenarios/NavigationSimulation/NavigationSimulation.scx
@@ -8,7 +8,7 @@
<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="RobotIK" instance="" package="RobotComponents" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <!-- <application name="RobotIK" instance="" package="RobotComponents" 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"/>
diff --git a/scenarios/PlatformNavigation/config/VisionMemory.cfg b/scenarios/PlatformNavigation/config/VisionMemory.cfg
index 5f13a29759a7d4422348928fda6b936d88af7516..9c59a9f6c9ab47cdf2a3a1a81ab015c1e6fccfaa 100644
--- a/scenarios/PlatformNavigation/config/VisionMemory.cfg
+++ b/scenarios/PlatformNavigation/config/VisionMemory.cfg
@@ -218,6 +218,15 @@
# ArmarX.VisionMemory.mem.ltm..buffer.storeFreq = 10
+# ArmarX.VisionMemory.mem.ltm.depthImageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.depthImageExtractor.Enabled = true
+
+
# ArmarX.VisionMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
@@ -227,6 +236,24 @@
# ArmarX.VisionMemory.mem.ltm.enabled = false
+# ArmarX.VisionMemory.mem.ltm.exrConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.exrConverter.Enabled = true
+
+
+# ArmarX.VisionMemory.mem.ltm.imageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.imageExtractor.Enabled = true
+
+
# ArmarX.VisionMemory.mem.ltm.memFreqFilter.Enabled:
# Attributes:
# - Default: false
@@ -244,20 +271,63 @@
# ArmarX.VisionMemory.mem.ltm.memFreqFilter.WaitingTime = -1
-# ArmarX.VisionMemory.mem.ltm.memSnapFilter.WaitingTime: Waiting time in MS after each Entity update.
+# ArmarX.VisionMemory.mem.ltm.pngConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.pngConverter.Enabled = true
+
+
+# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
+
+
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.Enabled = false
+
+
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
# Attributes:
# - Default: -1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.VisionMemory.mem.ltm.memSnapFilter.WaitingTime = -1
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
-# ArmarX.VisionMemory.mem.ltm.storagepath: The path to the memory storage.
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.Enabled:
# Attributes:
-# - Default: /tmp/MemoryExport
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.Enabled = false
+
+
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
+
+
+# ArmarX.VisionMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
+# Attributes:
+# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
-# ArmarX.VisionMemory.mem.ltm.storagepath = /tmp/MemoryExport
+# ArmarX.VisionMemory.mem.ltm.storagepath = Default value not mapped.
# ArmarX.VisionMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/PlatformNavigation/config/example_client.cfg b/scenarios/PlatformNavigation/config/example_client.cfg
index 384be319cc3e5b662d0bf1e38495746cd70b5ff4..e2d47a84b10811dfed3d8ff9a832c4b53621f69f 100644
--- a/scenarios/PlatformNavigation/config/example_client.cfg
+++ b/scenarios/PlatformNavigation/config/example_client.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# 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)
+# 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
@@ -102,12 +102,86 @@
# 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.descriptionSegment:
+# Attributes:
+# - Default: Description
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleClient.mem.robot_state.descriptionSegment = Description
+
+
+# ArmarX.ExampleClient.mem.robot_state.localizationSegment:
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleClient.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.ExampleClient.mem.robot_state.proprioceptionSegment:
+# Attributes:
+# - Default: Proprioception
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleClient.mem.robot_state.proprioceptionSegment = Proprioception
+
+
+# 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.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.ExampleClient.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleClient.tpc.sub.MemoryListener = MemoryUpdates
# 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;...
diff --git a/scenarios/PlatformNavigation/config/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index d02930d49a3c59772e6a4d3a4ba77acad517f843..604df43db8f358d79669b6faffee43647e91af67 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -158,6 +158,15 @@
# ArmarX.NavigationMemory.mem.ltm..buffer.storeFreq = 10
+# ArmarX.NavigationMemory.mem.ltm.depthImageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.depthImageExtractor.Enabled = true
+
+
# ArmarX.NavigationMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
@@ -167,7 +176,34 @@
# ArmarX.NavigationMemory.mem.ltm.enabled = false
-# ArmarX.NavigationMemory.mem.ltm.memFreqFilter.WaitingTime: Withdraw time in MS after each LTM update.
+# ArmarX.NavigationMemory.mem.ltm.exrConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.exrConverter.Enabled = true
+
+
+# ArmarX.NavigationMemory.mem.ltm.imageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.imageExtractor.Enabled = true
+
+
+# ArmarX.NavigationMemory.mem.ltm.memFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.memFreqFilter.Enabled = false
+
+
+# ArmarX.NavigationMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
# Attributes:
# - Default: -1
# - Case sensitivity: yes
@@ -175,20 +211,63 @@
# ArmarX.NavigationMemory.mem.ltm.memFreqFilter.WaitingTime = -1
-# ArmarX.NavigationMemory.mem.ltm.memSnapFilter.WaitingTime: Withdraw time in MS after each Entity update.
+# ArmarX.NavigationMemory.mem.ltm.pngConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.pngConverter.Enabled = true
+
+
+# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
+
+
+# ArmarX.NavigationMemory.mem.ltm.snapEqFilter.Enabled:
# Attributes:
-# - Default: 1000
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.memSnapFilter.WaitingTime = 1000
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.snapEqFilter.Enabled = false
-# ArmarX.NavigationMemory.mem.ltm.storagepath: The path to the memory storage.
+# ArmarX.NavigationMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
# Attributes:
-# - Default: /tmp/MemoryExport
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NavigationMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
+
+
+# ArmarX.NavigationMemory.mem.ltm.snapFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.snapFreqFilter.Enabled = false
+
+
+# ArmarX.NavigationMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NavigationMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
+
+
+# ArmarX.NavigationMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
+# Attributes:
+# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.storagepath = /tmp/MemoryExport
+# ArmarX.NavigationMemory.mem.ltm.storagepath = Default value not mapped.
# ArmarX.NavigationMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index d3a831f205ddd47ddb6533bd2891ea99a8cdadc8..4f2c5e40eb5b4bf29345341a9c49c5866f23f45a 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -310,6 +310,22 @@ ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
# ArmarX.Navigator.mem.robot_state.proprioceptionSegment = Proprioception
+# ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment: No Description
+# Attributes:
+# - Default: OccupancyGrid
+# - Case sensitivity: no
+# - Required: no
+ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment = OccupancyGrid
+
+
+# ArmarX.Navigator.mem.vision.occupancy_grid.Memory: No Description
+# Attributes:
+# - Default: Vision
+# - Case sensitivity: no
+# - Required: no
+ArmarX.Navigator.mem.vision.occupancy_grid.Memory = Vision
+
+
# 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:
@@ -412,13 +428,3 @@ ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
ArmarX.Verbosity = Verbose
-# ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment:
-# Attributes:
-ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment = OccupancyGrid
-
-
-# ArmarX.Navigator.mem.vision.occupancy_grid.Memory:
-# Attributes:
-ArmarX.Navigator.mem.vision.occupancy_grid.Memory = Vision
-
-
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 08eac501308e82fe25d77fed6134d6ad2f11d728..890ffc259c094f90e8398051aa68ddb815b22911 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -44,6 +44,7 @@
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
@@ -163,7 +164,7 @@ namespace armarx::navigation::components
// initialize scene
ARMARX_TRACE;
- updateContext();
+ initializeScene();
executor = server::PlatformUnitExecutor(platformUnit);
@@ -175,7 +176,7 @@ namespace armarx::navigation::components
robotStateUpdateTask = new PeriodicTask<Navigator>(
- this, &Navigator::updateRobot, 10, false, "RobotStateUpdateTask");
+ this, &Navigator::updateRobot, params.robotUpdatePeriodMs, false, "RobotStateUpdateTask");
robotStateUpdateTask->start();
navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
@@ -224,8 +225,18 @@ namespace armarx::navigation::components
{
}
+ VirtualRobot::RobotPtr
+ components::Navigator::getRobot()
+ {
+ ARMARX_TRACE;
+ auto robot = virtualRobotReader.getSynchronizedRobot(params.robotName, armarx::core::time::Clock::Now(), VirtualRobot::RobotIO::RobotDescription::eCollisionModel, true );
+
+ ARMARX_CHECK_NOT_NULL(robot);
+ return robot;
+ }
+
void
- components::Navigator::updateContext()
+ components::Navigator::initializeScene()
{
ARMARX_TRACE;
@@ -238,9 +249,9 @@ namespace armarx::navigation::components
{
scene.staticScene = staticScene();
- cv::Mat1f mat;
- cv::eigen2cv(scene.staticScene->costmap->getGrid(), mat);
- cv::imwrite("/tmp/grid.exr", mat);
+ // cv::Mat1f mat;
+ // cv::eigen2cv(scene.staticScene->costmap->getGrid(), mat);
+ // cv::imwrite("/tmp/grid.exr", mat);
}
}
@@ -305,7 +316,6 @@ namespace armarx::navigation::components
try
{
- updateContext();
navigators.at(callerId).moveTo(convert(waypoints),
core::NavigationFrameNames.from_name(navigationMode));
}
@@ -337,7 +347,6 @@ namespace armarx::navigation::components
ARMARX_CHECK(navigators.count(callerId) > 0)
<< "Navigator config for caller `" << callerId << "` not registered!";
- updateContext();
visualizeSPFA();
navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
}
@@ -355,7 +364,6 @@ namespace armarx::navigation::components
ARMARX_CHECK(navigators.count(callerId) > 0)
<< "Navigator config for caller `" << callerId << "` not registered!";
- updateContext();
navigators.at(callerId).moveTowards(direction,
core::NavigationFrameNames.from_name(navigationMode));
}
@@ -380,6 +388,7 @@ namespace armarx::navigation::components
void
components::Navigator::stop(const std::string& configId, const Ice::Current&)
{
+ // FIXME make sure that event is emitted
ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
<< "Navigator config for caller `" << configId << "` not registered!";
@@ -523,9 +532,10 @@ namespace armarx::navigation::components
ARMARX_INFO << objects->getSize() << " objects in the scene";
ARMARX_INFO << "Creating costmap";
+ ARMARX_CHECK_NOT_NULL(scene.robot);
algorithms::CostmapBuilder costmapBuilder(
- getRobot(),
+ scene.robot,
objects,
algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
"Platform-navigation-colmodel");
@@ -626,38 +636,24 @@ namespace armarx::navigation::components
ARMARX_INFO << "Done.";
}
- VirtualRobot::RobotPtr
- components::Navigator::getRobot()
- {
- ARMARX_TRACE;
-
- auto robot = RemoteRobot::createLocalCloneFromFile(
- getRobotStateComponent(), VirtualRobot::RobotIO::RobotDescription::eCollisionModel);
- // auto robot = RemoteRobot::createLocalClone(getRobotStateComponent());
- ARMARX_CHECK_NOT_NULL(robot);
-
- // ARMARX_INFO << "Resetting robot";
- // robot = VirtualRobot::RobotIO::loadRobot(robot->getFilename());
-
- RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
- return robot;
- }
void
components::Navigator::updateRobot()
{
- ARMARX_TRACE;
- synchronizeLocalClone(scene.robot);
+ const auto timestamp = armem::Time::Now();
- auto robotDescription = virtualRobotReader.queryDescription("Armar6", armem::Time::Now());
+ ARMARX_TRACE;
+ virtualRobotReader.synchronizeRobot(*scene.robot, timestamp);
+ ARMARX_TRACE;
+ const auto robotDescription = virtualRobotReader.queryDescription(params.robotName, timestamp);
ARMARX_CHECK(robotDescription.has_value());
// synchronizing platform state
+ ARMARX_TRACE;
const std::optional<armem::robot::PlatformState> platformState =
virtualRobotReader.queryPlatformState(robotDescription.value(), armem::Time::Now());
-
ARMARX_CHECK(platformState.has_value());
// TODO / FIXME make this thread safe!
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 326418c3c01540df4f038a60339b6c3a4bb1687e..3e5aa8824e7485191323ca712be593a4e5f5edb0 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -82,7 +82,6 @@ namespace armarx::navigation::components
virtual public Component,
virtual public client::NavigatorInterface,
virtual public ObjectPoseClientPluginUser,
- virtual public RobotStateComponentPluginUser,
virtual public ArVizComponentPluginUser,
virtual public armem::ListeningClientPluginUser
{
@@ -152,11 +151,11 @@ namespace armarx::navigation::components
/// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
- void updateContext();
+ void initializeScene();
core::StaticScene staticScene();
- VirtualRobot::RobotPtr getRobot();
+ [[nodiscard]] VirtualRobot::RobotPtr getRobot();
void updateRobot();
server::Navigator* activeNavigator();
@@ -213,6 +212,10 @@ namespace armarx::navigation::components
struct Parameters
{
float occupiedGridThreshold{0.55F};
+
+ std::string robotName = "Armar6";
+
+ int robotUpdatePeriodMs = 10;
};
Parameters params;
diff --git a/source/armarx/navigation/components/example_client/CMakeLists.txt b/source/armarx/navigation/components/example_client/CMakeLists.txt
index 784e3bfbb242e6bc540bea245112603693bdeb4f..01f000d5e96b1135fb133149e40dfd3f5116c9d6 100644
--- a/source/armarx/navigation/components/example_client/CMakeLists.txt
+++ b/source/armarx/navigation/components/example_client/CMakeLists.txt
@@ -9,6 +9,8 @@ armarx_add_component(example_client
# ArmarXGuiComponentPlugins # For RemoteGui plugin. RobotAPI
# RobotAPICore RobotAPIInterfaces RobotAPIArmarXObjects
# RobotAPIComponentPlugins # For ArViz and other plugins. Navigation
+ armem_robot_state
+ armem_robot
armarx_navigation::client
Simox::SimoxUtility
Eigen3::Eigen
diff --git a/source/armarx/navigation/components/example_client/Component.cpp b/source/armarx/navigation/components/example_client/Component.cpp
index 51f8a6d617e8531a177618714a3323f96e36d117..56404a58d91a9a8069187941d30b590347f661e9 100644
--- a/source/armarx/navigation/components/example_client/Component.cpp
+++ b/source/armarx/navigation/components/example_client/Component.cpp
@@ -24,10 +24,20 @@
#include "Component.h"
#include <chrono>
+#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/client/PathBuilder.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
@@ -36,7 +46,7 @@
namespace armarx::navigation::components::example_client
{
- Component::Component()
+ Component::Component() : virtualRobotReader(memoryNameSystem())
{
// pass
}
@@ -58,7 +68,9 @@ namespace armarx::navigation::components::example_client
void
Component::onConnectComponent()
{
- task = new armarx::RunningTask<Component>(this, &Component::exampleNavigationComplex);
+ virtualRobotReader.connect();
+
+ task = new armarx::RunningTask<Component>(this, &Component::exampleNavigationPointToPoint);
task->start();
}
@@ -264,6 +276,87 @@ namespace armarx::navigation::components::example_client
}
+ void
+ Component::exampleNavigationPointToPoint()
+ {
+ ARMARX_INFO << "Demonstrating point to point navigation";
+
+ using namespace std::chrono_literals;
+
+ // 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::Point2PointParams())
+ .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
+
+
+ std::this_thread::sleep_for(1s);
+
+ ARMARX_INFO << "Moving to goal pose";
+ // Start moving to goal position using above config.
+
+ auto robot =
+ virtualRobotReader.getSynchronizedRobot(properties.robotName, armarx::Clock::Now());
+ ARMARX_CHECK_NOT_NULL(robot);
+
+ const core::Pose initialPose(robot->getGlobalPose());
+
+ const core::Pose goal1 =
+ initialPose * Eigen::Translation3f(properties.relativeMovement, 0, 0); // to the right
+
+ const core::Pose goal2 =
+ goal1 * Eigen::Translation3f(0, properties.relativeMovement, 0) *
+ Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitZ()); // forward + 90° to the left
+
+ const core::Pose goal3 =
+ goal2 * Eigen::Translation3f(-properties.relativeMovement, 0, 0) *
+ Eigen::AngleAxisf(-M_PI_2f32,
+ Eigen::Vector3f::UnitZ()); // left and back to initial orientation
+
+ const core::Pose& finalGoal = initialPose;
+
+ const auto moveToP2P = [&](const core::Pose& targetPose)
+ {
+ // define movement
+ client::PathBuilder pathBuilder;
+ pathBuilder.add(targetPose,
+ armarx::navigation::client::GlobalPlanningStrategy::Point2Point);
+
+ // trigger movement
+ ARMARX_INFO << "Starting execution";
+ getNavigator().moveTo(pathBuilder, core::NavigationFrame::Absolute);
+
+ // wait for navigator to reach the target
+ ARMARX_INFO << "Waiting for navigator to reach the target";
+ armarx::navigation::client::StopEvent se = getNavigator().waitForStop();
+
+ // In case of any failure, the following will raise an exception to avoid subsequent navigation requests to be executed.
+ ARMARX_CHECK(se.isGoalReachedEvent());
+ };
+
+ ARMARX_IMPORTANT << "Moving to goal 1";
+ moveToP2P(goal1);
+
+ ARMARX_IMPORTANT << "Moving to goal 2";
+ moveToP2P(goal2);
+
+ ARMARX_IMPORTANT << "Moving to goal 3";
+ moveToP2P(goal3);
+
+ ARMARX_IMPORTANT << "Moving to final goal";
+ moveToP2P(finalGoal);
+
+ ARMARX_IMPORTANT << "Done. Pausing movement.";
+ getNavigator().pause();
+ }
+
+
void
Component::goalReached()
{
@@ -276,6 +369,16 @@ namespace armarx::navigation::components::example_client
{
armarx::PropertyDefinitionsPtr def =
new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+
+ def->optional(properties.robotName, "robotName");
+
+ def->optional(properties.relativeMovement,
+ "relativeMovement",
+ "The distance between two target poses [mm]");
+
+ virtualRobotReader.registerPropertyDefinitions(def);
+
+
return def;
}
diff --git a/source/armarx/navigation/components/example_client/Component.h b/source/armarx/navigation/components/example_client/Component.h
index 1864c62e8786f7470150ca5cad79e4b7768242a4..aaa629992d11fd4d873ab2777a3657df7538e0e8 100644
--- a/source/armarx/navigation/components/example_client/Component.h
+++ b/source/armarx/navigation/components/example_client/Component.h
@@ -27,6 +27,8 @@
#include <ArmarXCore/util/tasks.h>
#include <armarx/navigation/client.h>
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/libraries/armem/client/plugins.h>
namespace armarx::navigation::components::example_client
@@ -45,7 +47,8 @@ namespace armarx::navigation::components::example_client
*/
class Component :
virtual public armarx::Component,
- virtual public armarx::navigation::client::ComponentPluginUser
+ virtual public armarx::navigation::client::ComponentPluginUser,
+ virtual public armem::ListeningClientPluginUser
{
public:
@@ -74,10 +77,31 @@ namespace armarx::navigation::components::example_client
void exampleNavigation();
void exampleNavigationComplex();
+ /**
+ * @brief demonstrates point to point movement.
+ *
+ * The robot will move along a rectange in the following way:
+ *
+ * (1) move to the right
+ * (2) move forward and turn 90° to the left
+ * (3) move left and turn back 90° to the right
+ * (4) backward (this is the initial pose)
+ *
+ */
+ void exampleNavigationPointToPoint();
+
private:
void goalReached();
+ struct{
+ std::string robotName = "Armar6";
+ float relativeMovement = 200; // [mm]
+ } properties;
+
armarx::RunningTask<Component>::pointer_type task;
+
+ armem::robot_state::VirtualRobotReader virtualRobotReader;
+
};
} // namespace armarx::navigation::components::example_client