From b56b1b8d767007ab39f0ec98e279238fd163697d Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 16 Nov 2023 14:20:58 +0100
Subject: [PATCH] Add NavigateToNamedLocation skill to skill provider
---
.../navigation_skill_provider/Component.cpp | 29 ++++++++++++++-----
.../navigation_skill_provider/Component.h | 3 ++
2 files changed, 25 insertions(+), 7 deletions(-)
diff --git a/source/armarx/navigation/components/navigation_skill_provider/Component.cpp b/source/armarx/navigation/components/navigation_skill_provider/Component.cpp
index 06466322..3cb0bdee 100644
--- a/source/armarx/navigation/components/navigation_skill_provider/Component.cpp
+++ b/source/armarx/navigation/components/navigation_skill_provider/Component.cpp
@@ -25,21 +25,16 @@
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include <armarx/navigation/location/constants.h>
#include <armarx/navigation/skills/GuideHumanToRoom.h>
#include <armarx/navigation/skills/MoveRelativePlanar.h>
#include <armarx/navigation/skills/MoveXMeters.h>
#include <armarx/navigation/skills/NavigateTo.h>
#include <armarx/navigation/skills/NavigateToLocation.h>
+#include <armarx/navigation/skills/NavigateToNamedLocation.h>
#include <armarx/navigation/skills/RotateXDegrees.h>
#include <armarx/navigation/skills/constants.h>
-// Include headers you only need in function definitions in the .cpp.
-
-// #include <Eigen/Core>
-
-// #include <SimoxUtility/color/Color.h>
-
-
namespace armarx::navigation::components::navigation_skill_provider
{
Component::Component()
@@ -61,6 +56,17 @@ namespace armarx::navigation::components::navigation_skill_provider
def->required(properties.robotName, "RobotName", "Default robot name.");
+ auto optionalSubSkillID =
+ [&def](armarx::skills::SkillID& skillID, const std::string& nameBase)
+ {
+ ARMARX_CHECK(skillID.providerId.has_value());
+ def->optional(skillID.providerId->providerName, nameBase + ".providerName");
+ def->optional(skillID.skillName, nameBase + ".skillName");
+ };
+
+ optionalSubSkillID(properties.navigateToNamedLocation.subSkillIDs.navigateToLocation,
+ "p.navigateToNamedLocation.subSkillIDs.navigateToLocation");
+
return def;
}
@@ -97,6 +103,15 @@ namespace armarx::navigation::components::navigation_skill_provider
addSkillFactory<skills::NavigateToLocation>(srv);
}
+ {
+ skills::NavigateToNamedLocation::Services services{
+ .locationReader =
+ memoryNameSystem().getReader(armarx::navigation::location::coreSegmentID),
+ };
+ addSkillFactory<skills::NavigateToNamedLocation>(properties.navigateToNamedLocation,
+ services);
+ }
+
{
skills::MoveXMeters::Services srv{
.iceNavigator = iceNavigator,
diff --git a/source/armarx/navigation/components/navigation_skill_provider/Component.h b/source/armarx/navigation/components/navigation_skill_provider/Component.h
index 5862cc84..086efa45 100644
--- a/source/armarx/navigation/components/navigation_skill_provider/Component.h
+++ b/source/armarx/navigation/components/navigation_skill_provider/Component.h
@@ -37,6 +37,7 @@
#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>
+#include <armarx/navigation/skills/NavigateToNamedLocation.h>
#include <armarx/navigation/skills/forward_declarations.h>
namespace armarx::navigation::components::navigation_skill_provider
@@ -109,6 +110,8 @@ namespace armarx::navigation::components::navigation_skill_provider
struct Properties
{
std::string robotName;
+
+ skills::NavigateToNamedLocation::Properties navigateToNamedLocation;
};
Properties properties;
--
GitLab