From c1ffafa3f51001fd57fb0e49500831f00ad06815 Mon Sep 17 00:00:00 2001
From: Fabian Peller <fabian.peller-konrad@kit.edu>
Date: Wed, 18 Oct 2023 10:49:40 +0200
Subject: [PATCH 1/2] changes related to robotapi skills/dev
---
.../Component.cpp | 13 +++-----
.../navigation_skill_provider/Component.cpp | 29 ++++++-----------
.../navigation_skill_provider/Component.h | 9 ------
.../navigation/memory/client/graph/Reader.cpp | 6 ++--
.../navigation/memory/client/graph/Reader.h | 2 +-
.../navigation/skills/GuideHumanToRoom.cpp | 17 +++-------
.../navigation/skills/GuideHumanToRoom.h | 31 +++++++++----------
.../armarx/navigation/skills/MoveXMeters.cpp | 6 ++--
source/armarx/navigation/skills/MoveXMeters.h | 14 ++++-----
.../navigation/skills/NavigateRelativeSkill.h | 18 +++++------
.../armarx/navigation/skills/NavigateTo.cpp | 30 +++++++-----------
source/armarx/navigation/skills/NavigateTo.h | 24 +++++++-------
.../navigation/skills/NavigateToLocation.cpp | 31 +++++++------------
.../navigation/skills/NavigateToLocation.h | 22 ++++++-------
.../navigation/skills/RotateXDegrees.cpp | 4 +--
.../armarx/navigation/skills/RotateXDegrees.h | 14 ++++-----
16 files changed, 108 insertions(+), 162 deletions(-)
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 5bab4b0d..0f356abd 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
@@ -96,10 +96,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
}
Component::Component() :
- robotReader(memoryNameSystem()),
- costmapReader(memoryNameSystem()),
- costmapWriter(memoryNameSystem()),
- laserScannerFeaturesReader(memoryNameSystem())
+ robotReader(), costmapReader(), costmapWriter(), laserScannerFeaturesReader()
{
}
@@ -145,14 +142,14 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// connect memory readers and writers
ARMARX_INFO << "Connecting to robot state memory";
- robotReader.connect();
+ robotReader.connect(memoryNameSystem());
ARMARX_INFO << "Connecting to costmap segments";
- costmapReader.connect();
- costmapWriter.connect();
+ costmapReader.connect(memoryNameSystem());
+ costmapWriter.connect(memoryNameSystem());
ARMARX_INFO << "Connecting to laser scanner features segment";
- laserScannerFeaturesReader.connect();
+ laserScannerFeaturesReader.connect(memoryNameSystem());
ARMARX_CHECK(readStaticCostmap());
diff --git a/source/armarx/navigation/components/navigation_skill_provider/Component.cpp b/source/armarx/navigation/components/navigation_skill_provider/Component.cpp
index 897fd3c3..90226a4a 100644
--- a/source/armarx/navigation/components/navigation_skill_provider/Component.cpp
+++ b/source/armarx/navigation/components/navigation_skill_provider/Component.cpp
@@ -85,12 +85,6 @@ 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_.moveXMeters = addSkill<skills::MoveXMeters>();
- skills_.rotateXDegrees = addSkill<skills::RotateXDegrees>();
- skills_.guideHumanToRoom = addSkill<skills::GuideHumanToRoom>();
}
void
@@ -102,35 +96,31 @@ namespace armarx::navigation::components::navigation_skill_provider
{
- ARMARX_CHECK_NOT_NULL(skills_.navigateTo);
-
skills::NavigateTo::Services srv{.iceNavigator = iceNavigator,
.memoryNameSystem = memoryNameSystem()};
- skills_.navigateTo->connect(srv);
+ addSkillFactory<skills::NavigateTo>(srv);
}
{
- ARMARX_CHECK_NOT_NULL(skills_.navigateToLocation);
-
skills::NavigateToLocation::Services srv{.iceNavigator = iceNavigator,
.memoryNameSystem = memoryNameSystem()};
- skills_.navigateToLocation->connect(srv);
+
+ addSkillFactory<skills::NavigateToLocation>(srv);
}
{
- ARMARX_CHECK_NOT_NULL(skills_.moveXMeters);
-
skills::MoveXMeters::Services srv{.iceNavigator = iceNavigator,
.memoryNameSystem = memoryNameSystem()};
- skills_.moveXMeters->connect(srv);
+
+ addSkillFactory<skills::MoveXMeters>(srv);
}
{
- ARMARX_CHECK_NOT_NULL(skills_.rotateXDegrees);
skills::RotateXDegrees::Services srv{.iceNavigator = iceNavigator,
.memoryNameSystem = memoryNameSystem()};
- skills_.rotateXDegrees->connect(srv);
+
+ addSkillFactory<skills::RotateXDegrees>(srv);
}
{
@@ -138,8 +128,6 @@ namespace armarx::navigation::components::navigation_skill_provider
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 =
@@ -147,7 +135,8 @@ namespace armarx::navigation::components::navigation_skill_provider
.costmapReader = costmapReaderPlugin->get(),
.roomsReader = roomsReaderPlugin->get(),
.arviz = arviz};
- skills_.guideHumanToRoom->connect(srv);
+
+ addSkillFactory<skills::GuideHumanToRoom>(srv);
}
diff --git a/source/armarx/navigation/components/navigation_skill_provider/Component.h b/source/armarx/navigation/components/navigation_skill_provider/Component.h
index 153df614..6fcb682e 100644
--- a/source/armarx/navigation/components/navigation_skill_provider/Component.h
+++ b/source/armarx/navigation/components/navigation_skill_provider/Component.h
@@ -109,15 +109,6 @@ namespace armarx::navigation::components::navigation_skill_provider
void drawBoxes(const Properties& p, viz::Client& arviz);
*/
- struct Skills
- {
- skills::NavigateTo* navigateTo;
- skills::NavigateToLocation* navigateToLocation;
- skills::RotateXDegrees* rotateXDegrees;
- skills::MoveXMeters* moveXMeters;
- skills::GuideHumanToRoom* guideHumanToRoom;
- } skills_;
-
private:
static const std::string defaultName;
diff --git a/source/armarx/navigation/memory/client/graph/Reader.cpp b/source/armarx/navigation/memory/client/graph/Reader.cpp
index dd92cfa8..4211de57 100644
--- a/source/armarx/navigation/memory/client/graph/Reader.cpp
+++ b/source/armarx/navigation/memory/client/graph/Reader.cpp
@@ -196,7 +196,7 @@ namespace armarx::navigation::memory::client::graph
}
void
- Reader::connect()
+ Reader::connect(armarx::armem::client::MemoryNameSystem& mns)
{
// for the sake of completion ...
// armem::client::util::SimpleReaderBase::connect();
@@ -208,11 +208,11 @@ namespace armarx::navigation::memory::client::graph
try
{
ARMARX_INFO << "graphs ...";
- memoryReaderGraphs = memoryNameSystem.useReader(
+ memoryReaderGraphs = mns.useReader(
armem::MemoryID().withMemoryName(navigation::graph::coreSegmentID.memoryName));
ARMARX_INFO << "locations ...";
- memoryReaderLocations = memoryNameSystem.useReader(
+ memoryReaderLocations = mns.useReader(
armem::MemoryID().withMemoryName(navigation::location::coreSegmentID.memoryName));
ARMARX_IMPORTANT << "Reader: Connected to memory '"
diff --git a/source/armarx/navigation/memory/client/graph/Reader.h b/source/armarx/navigation/memory/client/graph/Reader.h
index 7f04a407..cb6b73ca 100644
--- a/source/armarx/navigation/memory/client/graph/Reader.h
+++ b/source/armarx/navigation/memory/client/graph/Reader.h
@@ -44,7 +44,7 @@ namespace armarx::navigation::memory::client::graph
core::Location resolveLocationId(const std::string& locationId);
- void connect() override;
+ void connect(armarx::armem::client::MemoryNameSystem& mns) override;
protected:
diff --git a/source/armarx/navigation/skills/GuideHumanToRoom.cpp b/source/armarx/navigation/skills/GuideHumanToRoom.cpp
index 0b25333b..dbaa0097 100644
--- a/source/armarx/navigation/skills/GuideHumanToRoom.cpp
+++ b/source/armarx/navigation/skills/GuideHumanToRoom.cpp
@@ -20,15 +20,9 @@
#include <armarx/navigation/rooms/RoomNavigationTargetCreator.h>
#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
-
namespace armarx::navigation::skills
{
- GuideHumanToRoom::GuideHumanToRoom() : Base(DefaultSkillDescription())
- {
- }
-
- void
- GuideHumanToRoom::connect(const Services& srv)
+ GuideHumanToRoom::GuideHumanToRoom(const Services& srv) : Base(DefaultSkillDescription())
{
srv_.emplace(srv);
}
@@ -45,7 +39,7 @@ namespace armarx::navigation::skills
cfg.trajectoryController(
armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams{});
- const std::string configId = DefaultSkillDescription().skillName;
+ const std::string configId = DefaultSkillDescription().skillId.skillName;
// configure the `navigator` which provides a simplified and typed interface to the navigation server
memorySubscriber.emplace(configId, srv_->memoryNameSystem);
@@ -64,7 +58,7 @@ namespace armarx::navigation::skills
::armarx::skills::Skill::MainResult
GuideHumanToRoom::main(const Base::SpecializedMainInput& in)
{
- const core::Pose globalTarget = getTargetInFrontOfRoom(in.params.room);
+ const core::Pose globalTarget = getTargetInFrontOfRoom(in.parameters.room);
ARMARX_INFO << "moving to target " << VAROUT(globalTarget.matrix());
@@ -110,7 +104,6 @@ namespace armarx::navigation::skills
.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
}
-
void
GuideHumanToRoom::onStopRequested()
{
@@ -162,8 +155,8 @@ namespace armarx::navigation::skills
rooms::RoomNavigationTargetCreator::Params algoParams;
rooms::RoomNavigationTargetCreator algo(algoParams);
- auto result =
- algo.getClosestPositionOutsideOfRoom(costmap, room, global_T_robot.translation(), &srv_->arviz);
+ auto result = algo.getClosestPositionOutsideOfRoom(
+ costmap, room, global_T_robot.translation(), &srv_->arviz);
const core::Direction dirToRoom = result.global_P_roomEntry - result.global_P_robot;
diff --git a/source/armarx/navigation/skills/GuideHumanToRoom.h b/source/armarx/navigation/skills/GuideHumanToRoom.h
index ea0f954d..ce2f4e64 100644
--- a/source/armarx/navigation/skills/GuideHumanToRoom.h
+++ b/source/armarx/navigation/skills/GuideHumanToRoom.h
@@ -26,24 +26,24 @@
#include "RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
-#include <RobotAPI/libraries/skills/provider/Skill.h>
-#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.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/memory/client/costmap/Reader.h>
+#include <armarx/navigation/memory/client/rooms/Reader.h>
#include <armarx/navigation/skills/aron/GuideHumanToRoom.aron.generated.h>
#include <armarx/navigation/skills/constants.h>
namespace armarx::navigation::skills
{
- class GuideHumanToRoom : virtual public armarx::skills::SpecializedSkill<arondto::GuideHumanToRoomParams>
+ class GuideHumanToRoom :
+ virtual public armarx::skills::SimpleSpecializedSkill<arondto::GuideHumanToRoomParams>
{
public:
using Params = arondto::GuideHumanToRoomParams;
- using Base = ::armarx::skills::SpecializedSkill<Params>;
-
+ using Base = ::armarx::skills::SimpleSpecializedSkill<Params>;
struct Services
{
@@ -62,14 +62,13 @@ namespace armarx::navigation::skills
struct Properties
{
- std::string distanceToObstacleCostmapProvider = "distance_to_obstacle_costmap_provider"; // FIXME
+ std::string distanceToObstacleCostmapProvider =
+ "distance_to_obstacle_costmap_provider"; // FIXME
std::string roomsProvider = "R003"; // FIXME
std::string robotName = "Armar6"; // FIXME
};
- GuideHumanToRoom();
-
- void connect(const Services& srv);
+ GuideHumanToRoom(const Services& srv);
private:
@@ -85,11 +84,11 @@ namespace armarx::navigation::skills
static armarx::skills::SkillDescription
DefaultSkillDescription()
{
- return {constants::skill_names::GuideHumanToRoom,
- "",
- {},
- armarx::core::time::Duration(),
- Params::ToAronType()};
+ return armarx::skills::SkillDescription({constants::skill_names::GuideHumanToRoom},
+ "",
+ nullptr,
+ armarx::core::time::Duration(),
+ Params::ToAronType());
}
protected:
diff --git a/source/armarx/navigation/skills/MoveXMeters.cpp b/source/armarx/navigation/skills/MoveXMeters.cpp
index feddb278..72aa702f 100644
--- a/source/armarx/navigation/skills/MoveXMeters.cpp
+++ b/source/armarx/navigation/skills/MoveXMeters.cpp
@@ -3,15 +3,15 @@
namespace armarx::navigation::skills
{
- MoveXMeters::MoveXMeters() : Base(DefaultSkillDescription())
+ MoveXMeters::MoveXMeters(const Services& srv) : Base(srv, DefaultSkillDescription())
{
}
Eigen::Isometry3f
MoveXMeters::relativeTarget(const Base::SpecializedMainInput& in)
{
- Eigen::Vector3f scaledVector = in.params.distanceMillimeters * Eigen::Vector3f::UnitY();
- return Eigen::Isometry3f(Eigen::Translation3f(in.params.direction * scaledVector));
+ Eigen::Vector3f scaledVector = in.parameters.distanceMillimeters * Eigen::Vector3f::UnitY();
+ return Eigen::Isometry3f(Eigen::Translation3f(in.parameters.direction * scaledVector));
}
} // namespace armarx::navigation::skills
diff --git a/source/armarx/navigation/skills/MoveXMeters.h b/source/armarx/navigation/skills/MoveXMeters.h
index ed413beb..4828c962 100644
--- a/source/armarx/navigation/skills/MoveXMeters.h
+++ b/source/armarx/navigation/skills/MoveXMeters.h
@@ -13,7 +13,7 @@ namespace armarx::navigation::skills
public:
using Base = NavigateRelativeSkill<arondto::MoveXMetersParams>;
- MoveXMeters();
+ MoveXMeters(const Services& srv);
private:
Eigen::Isometry3f relativeTarget(const Base::SpecializedMainInput& in) override;
@@ -27,12 +27,12 @@ namespace armarx::navigation::skills
defaultParams.distanceMillimeters = 0;
defaultParams.direction = Eigen::Quaternion<float, 0>::Identity();
- return {.skillName = constants::skill_names::MoveXMeters,
- .description = "Relative movement in specified direction",
- .robots = {},
- .timeout = armarx::core::time::Duration::Minutes(1),
- .acceptedType = ParamType::ToAronType(),
- .defaultParams = defaultParams.toAron()};
+ return armarx::skills::SkillDescription(
+ armarx::skills::SkillID(constants::skill_names::MoveXMeters),
+ "Relative movement in specified direction",
+ defaultParams.toAron(),
+ armarx::core::time::Duration::Minutes(1),
+ ParamType::ToAronType());
}
};
diff --git a/source/armarx/navigation/skills/NavigateRelativeSkill.h b/source/armarx/navigation/skills/NavigateRelativeSkill.h
index 09de2688..af6a30e6 100644
--- a/source/armarx/navigation/skills/NavigateRelativeSkill.h
+++ b/source/armarx/navigation/skills/NavigateRelativeSkill.h
@@ -2,7 +2,7 @@
#include <future>
-#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
#include <armarx/navigation/client.h>
#include <armarx/navigation/client/services/MemorySubscriber.h>
@@ -39,11 +39,11 @@ namespace armarx::navigation::skills
// abstract base class of a relative movement skill
template <class AronT>
- class NavigateRelativeSkill : public armarx::skills::SpecializedSkill<AronT>
+ class NavigateRelativeSkill : public armarx::skills::SimpleSpecializedSkill<AronT>
{
public:
- using Base = armarx::skills::SpecializedSkill<AronT>;
+ using Base = armarx::skills::SimpleSpecializedSkill<AronT>;
struct Services
{
@@ -51,16 +51,12 @@ namespace armarx::navigation::skills
armem::client::MemoryNameSystem& memoryNameSystem;
};
- void
- connect(const Services& srv)
- {
- srv_.emplace(srv);
- }
-
NavigateRelativeSkill() = delete;
- NavigateRelativeSkill(const armarx::skills::SkillDescription& desc) : Base(desc)
+ NavigateRelativeSkill(const Services& srv, const armarx::skills::SkillDescription& desc) :
+ Base(desc)
{
+ srv_.emplace(srv);
}
virtual ~NavigateRelativeSkill() = default;
@@ -113,7 +109,7 @@ namespace armarx::navigation::skills
std::async(std::launch::async, [this]() { return navigator->waitForStop(); });
while (not is_ready(future))
{
- if (Base::checkWhetherSkillShouldStopASAP())
+ if (Base::shouldSkillTerminate())
{
ARMARX_INFO << "Skill was aborted by user " << Base::stopped << ", "
<< Base::timeoutReached;
diff --git a/source/armarx/navigation/skills/NavigateTo.cpp b/source/armarx/navigation/skills/NavigateTo.cpp
index e59817d9..19147b1c 100644
--- a/source/armarx/navigation/skills/NavigateTo.cpp
+++ b/source/armarx/navigation/skills/NavigateTo.cpp
@@ -1,10 +1,11 @@
#include "NavigateTo.h"
-#include <memory>
#include <future>
+#include <memory>
#include <Eigen/Core>
#include <Eigen/Geometry>
+
#include "ArmarXCore/core/logging/Logging.h"
#include <armarx/navigation/client/NavigationStackConfig.h>
@@ -45,12 +46,7 @@ namespace armarx::navigation::skills
}
}
- NavigateTo::NavigateTo() : Base(DefaultSkillDescription())
- {
- }
-
- void
- NavigateTo::connect(const Services& srv)
+ NavigateTo::NavigateTo(const Services& srv) : Base(DefaultSkillDescription())
{
srv_.emplace(srv);
}
@@ -62,14 +58,14 @@ namespace armarx::navigation::skills
armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams
trajControllerParams{};
- if (in.params.velocityLimitLinear.has_value())
+ if (in.parameters.velocityLimitLinear.has_value())
{
- trajControllerParams.limits.linear = in.params.velocityLimitLinear.value();
+ trajControllerParams.limits.linear = in.parameters.velocityLimitLinear.value();
}
- if (in.params.velocityLimitAngular.has_value())
+ if (in.parameters.velocityLimitAngular.has_value())
{
- trajControllerParams.limits.angular = in.params.velocityLimitAngular.value();
+ trajControllerParams.limits.angular = in.parameters.velocityLimitAngular.value();
}
ARMARX_INFO << "Configuring navigator to respect the velocity limits:";
@@ -82,7 +78,7 @@ namespace armarx::navigation::skills
cfg.globalPlanner(armarx::navigation::global_planning::SPFAParams{});
cfg.trajectoryController(trajControllerParams);
- const std::string configId = DefaultSkillDescription().skillName;
+ const std::string configId = DefaultSkillDescription().skillId.skillName;
// configure the `navigator` which provides a simplified and typed interface to the navigation server
memorySubscriber.emplace(configId, srv_->memoryNameSystem);
@@ -101,7 +97,7 @@ namespace armarx::navigation::skills
::armarx::skills::Skill::MainResult
NavigateTo::main(const Base::SpecializedMainInput& in)
{
- const Eigen::Isometry3f globalTarget(in.params.targetPose);
+ const Eigen::Isometry3f globalTarget(in.parameters.targetPose);
ARMARX_INFO << "moving to target " << VAROUT(globalTarget.matrix());
@@ -112,12 +108,10 @@ namespace armarx::navigation::skills
// Wait until goal is reached
ARMARX_INFO << "Waiting until goal is reached.";
- auto future =
- std::async(std::launch::async,
- [this]() { return navigator->waitForStop(); });
+ auto future = std::async(std::launch::async, [this]() { return navigator->waitForStop(); });
while (not is_ready(future))
{
- if (checkWhetherSkillShouldStopASAP())
+ if (shouldSkillTerminate())
{
ARMARX_INFO << "Skill was aborted by user " << stopped << ", " << timeoutReached;
navigator->stop();
@@ -128,7 +122,7 @@ namespace armarx::navigation::skills
auto se = future.get();
if (se)
{
- ARMARX_INFO << "Goal `" << Eigen::Isometry3f(in.params.targetPose).translation()
+ ARMARX_INFO << "Goal `" << Eigen::Isometry3f(in.parameters.targetPose).translation()
<< "`reached.";
}
else
diff --git a/source/armarx/navigation/skills/NavigateTo.h b/source/armarx/navigation/skills/NavigateTo.h
index 8c0225f3..d328553a 100644
--- a/source/armarx/navigation/skills/NavigateTo.h
+++ b/source/armarx/navigation/skills/NavigateTo.h
@@ -24,8 +24,8 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/time/Duration.h>
-#include <RobotAPI/libraries/skills/provider/Skill.h>
-#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
#include <armarx/navigation/client.h>
#include <armarx/navigation/client/services/MemorySubscriber.h>
@@ -34,12 +34,12 @@
namespace armarx::navigation::skills
{
- class NavigateTo : virtual public armarx::skills::SpecializedSkill<arondto::NavigateToParams>
+ class NavigateTo :
+ virtual public armarx::skills::SimpleSpecializedSkill<arondto::NavigateToParams>
{
public:
using Params = arondto::NavigateToParams;
- using Base = ::armarx::skills::SpecializedSkill<Params>;
-
+ using Base = ::armarx::skills::SimpleSpecializedSkill<Params>;
struct Services
{
@@ -47,9 +47,7 @@ namespace armarx::navigation::skills
armem::client::MemoryNameSystem& memoryNameSystem;
};
- NavigateTo();
-
- void connect(const Services& srv);
+ NavigateTo(const Services& srv);
private:
::armarx::skills::Skill::InitResult init(const Base::SpecializedInitInput& in) override;
@@ -63,11 +61,11 @@ namespace armarx::navigation::skills
static armarx::skills::SkillDescription
DefaultSkillDescription()
{
- return {constants::skill_names::NavigateTo,
- "",
- {},
- armarx::core::time::Duration::Days(42),
- Params::ToAronType()};
+ return armarx::skills::SkillDescription({constants::skill_names::NavigateTo},
+ "",
+ nullptr,
+ armarx::core::time::Duration::Days(42),
+ Params::ToAronType());
}
protected:
diff --git a/source/armarx/navigation/skills/NavigateToLocation.cpp b/source/armarx/navigation/skills/NavigateToLocation.cpp
index a4236d4f..79dc59d8 100644
--- a/source/armarx/navigation/skills/NavigateToLocation.cpp
+++ b/source/armarx/navigation/skills/NavigateToLocation.cpp
@@ -1,7 +1,7 @@
#include "NavigateToLocation.h"
-#include <memory>
#include <future>
+#include <memory>
#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -17,7 +17,6 @@
#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
-
namespace armarx::navigation::skills
{
@@ -46,12 +45,7 @@ namespace armarx::navigation::skills
}
}
- NavigateToLocation::NavigateToLocation() : Base(DefaultSkillDescription())
- {
- }
-
- void
- NavigateToLocation::connect(const Services& srv)
+ NavigateToLocation::NavigateToLocation(const Services& srv) : Base(DefaultSkillDescription())
{
srv_.emplace(srv);
}
@@ -62,14 +56,14 @@ namespace armarx::navigation::skills
armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams
trajControllerParams{};
- if (in.params.velocityLimitLinear.has_value())
+ if (in.parameters.velocityLimitLinear.has_value())
{
- trajControllerParams.limits.linear = in.params.velocityLimitLinear.value();
+ trajControllerParams.limits.linear = in.parameters.velocityLimitLinear.value();
}
- if (in.params.velocityLimitAngular.has_value())
+ if (in.parameters.velocityLimitAngular.has_value())
{
- trajControllerParams.limits.angular = in.params.velocityLimitAngular.value();
+ trajControllerParams.limits.angular = in.parameters.velocityLimitAngular.value();
}
ARMARX_INFO << "Configuring navigator to respect the velocity limits:";
@@ -82,7 +76,7 @@ namespace armarx::navigation::skills
cfg.globalPlanner(armarx::navigation::global_planning::SPFAParams{});
cfg.trajectoryController(trajControllerParams);
- const std::string configId = DefaultSkillDescription().skillName;
+ const std::string configId = DefaultSkillDescription().skillId.skillName;
// configure the `navigator` which provides a simplified and typed interface to the navigation server
ARMARX_TRACE;
@@ -105,7 +99,7 @@ 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.parameters.location;
ARMARX_INFO << "moving to target " << VAROUT(location);
@@ -119,12 +113,10 @@ namespace armarx::navigation::skills
// Wait until goal is reached
ARMARX_INFO << "Waiting until goal is reached.";
- auto future =
- std::async(std::launch::async,
- [this]() { return navigator->waitForStop(); });
+ auto future = std::async(std::launch::async, [this]() { return navigator->waitForStop(); });
while (not is_ready(future))
{
- if (checkWhetherSkillShouldStopASAP())
+ if (shouldSkillTerminate())
{
ARMARX_INFO << "Skill was aborted by user";
navigator->stop();
@@ -137,7 +129,7 @@ namespace armarx::navigation::skills
{
ARMARX_INFO << "Goal `" << location << "`reached.";
return ::armarx::skills::Skill::MainResult{
- .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
+ .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
}
else
{
@@ -170,7 +162,6 @@ namespace armarx::navigation::skills
.status = ::armarx::skills::TerminatedSkillStatus::Failed};
}
-
void
NavigateToLocation::onStopRequested()
{
diff --git a/source/armarx/navigation/skills/NavigateToLocation.h b/source/armarx/navigation/skills/NavigateToLocation.h
index b7dbf92b..3f76a74d 100644
--- a/source/armarx/navigation/skills/NavigateToLocation.h
+++ b/source/armarx/navigation/skills/NavigateToLocation.h
@@ -24,8 +24,8 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/time/Duration.h>
-#include <RobotAPI/libraries/skills/provider/Skill.h>
-#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
#include <armarx/navigation/client.h>
#include <armarx/navigation/client/services/MemorySubscriber.h>
@@ -35,11 +35,11 @@
namespace armarx::navigation::skills
{
class NavigateToLocation :
- virtual public armarx::skills::SpecializedSkill<arondto::NavigateToLocationParams>
+ virtual public armarx::skills::SimpleSpecializedSkill<arondto::NavigateToLocationParams>
{
public:
using Params = arondto::NavigateToLocationParams;
- using Base = ::armarx::skills::SpecializedSkill<Params>;
+ using Base = ::armarx::skills::SimpleSpecializedSkill<Params>;
struct Services
{
@@ -48,9 +48,7 @@ namespace armarx::navigation::skills
armem::client::MemoryNameSystem& memoryNameSystem;
};
- NavigateToLocation();
-
- void connect(const Services& srv);
+ NavigateToLocation(const Services& srv);
private:
::armarx::skills::Skill::InitResult init(const Base::SpecializedInitInput& in) override;
@@ -68,11 +66,11 @@ namespace armarx::navigation::skills
defaultParameters.location = "";
defaultParameters.velocityLimitAngular = std::nullopt;
defaultParameters.velocityLimitLinear = std::nullopt;
- return {constants::skill_names::NavigateToLocation,
- "",
- {},
- armarx::core::time::Duration::Days(42),
- Params::ToAronType(), defaultParameters.toAron()};
+ return armarx::skills::SkillDescription({constants::skill_names::NavigateToLocation},
+ "",
+ defaultParameters.toAron(),
+ armarx::core::time::Duration::Days(42),
+ Params::ToAronType());
}
protected:
diff --git a/source/armarx/navigation/skills/RotateXDegrees.cpp b/source/armarx/navigation/skills/RotateXDegrees.cpp
index 14a083d1..2443cd5a 100644
--- a/source/armarx/navigation/skills/RotateXDegrees.cpp
+++ b/source/armarx/navigation/skills/RotateXDegrees.cpp
@@ -5,14 +5,14 @@
namespace armarx::navigation::skills
{
- RotateXDegrees::RotateXDegrees() : Base(DefaultSkillDescription())
+ RotateXDegrees::RotateXDegrees(const Services& srv) : Base(srv, DefaultSkillDescription())
{
}
Eigen::Isometry3f
RotateXDegrees::relativeTarget(const Base::SpecializedMainInput& in)
{
- const float rotation = simox::math::deg_to_rad(in.params.rotationDegrees);
+ const float rotation = simox::math::deg_to_rad(in.parameters.rotationDegrees);
return Eigen::Isometry3f(Eigen::AngleAxisf(rotation, Eigen::Vector3f::UnitZ()));
}
diff --git a/source/armarx/navigation/skills/RotateXDegrees.h b/source/armarx/navigation/skills/RotateXDegrees.h
index 15622cf8..f874648d 100644
--- a/source/armarx/navigation/skills/RotateXDegrees.h
+++ b/source/armarx/navigation/skills/RotateXDegrees.h
@@ -13,7 +13,7 @@ namespace armarx::navigation::skills
public:
using Base = NavigateRelativeSkill<arondto::RotateXDegreesParams>;
- RotateXDegrees();
+ RotateXDegrees(const Services& srv);
private:
Eigen::Isometry3f relativeTarget(const Base::SpecializedMainInput& in) override;
@@ -26,12 +26,12 @@ namespace armarx::navigation::skills
ParamType defaultParams;
defaultParams.rotationDegrees = 0;
- return {.skillName = constants::skill_names::RotateXDegrees,
- .description = "Rotate around the Z-axis by a specified degree value",
- .robots = {},
- .timeout = armarx::core::time::Duration::Minutes(1),
- .acceptedType = ParamType::ToAronType(),
- .defaultParams = defaultParams.toAron()};
+ return armarx::skills::SkillDescription(
+ {constants::skill_names::RotateXDegrees},
+ "Rotate around the Z-axis by a specified degree value",
+ defaultParams.toAron(),
+ armarx::core::time::Duration::Minutes(1),
+ ParamType::ToAronType());
}
};
--
GitLab
From 650d7908a080d8cd8b5a8ce31cba413f954cdaa5 Mon Sep 17 00:00:00 2001
From: Fabian Peller <fabian.peller-konrad@kit.edu>
Date: Wed, 18 Oct 2023 12:55:32 +0200
Subject: [PATCH 2/2] changes related to robotapi skills/dev
---
source/armarx/navigation/skills/GuideHumanToRoom.h | 10 +++++-----
source/armarx/navigation/skills/MoveXMeters.h | 12 ++++++------
source/armarx/navigation/skills/NavigateTo.h | 10 +++++-----
source/armarx/navigation/skills/NavigateToLocation.h | 11 ++++++-----
source/armarx/navigation/skills/RotateXDegrees.h | 12 ++++++------
5 files changed, 28 insertions(+), 27 deletions(-)
diff --git a/source/armarx/navigation/skills/GuideHumanToRoom.h b/source/armarx/navigation/skills/GuideHumanToRoom.h
index ce2f4e64..a73bcd5a 100644
--- a/source/armarx/navigation/skills/GuideHumanToRoom.h
+++ b/source/armarx/navigation/skills/GuideHumanToRoom.h
@@ -84,11 +84,11 @@ namespace armarx::navigation::skills
static armarx::skills::SkillDescription
DefaultSkillDescription()
{
- return armarx::skills::SkillDescription({constants::skill_names::GuideHumanToRoom},
- "",
- nullptr,
- armarx::core::time::Duration(),
- Params::ToAronType());
+ return armarx::skills::SkillDescription{
+ .skillId = {.skillName = constants::skill_names::GuideHumanToRoom},
+ .description = "",
+ .timeout = armarx::core::time::Duration(),
+ .parametersType = Params::ToAronType()};
}
protected:
diff --git a/source/armarx/navigation/skills/MoveXMeters.h b/source/armarx/navigation/skills/MoveXMeters.h
index 4828c962..3cffdc74 100644
--- a/source/armarx/navigation/skills/MoveXMeters.h
+++ b/source/armarx/navigation/skills/MoveXMeters.h
@@ -27,12 +27,12 @@ namespace armarx::navigation::skills
defaultParams.distanceMillimeters = 0;
defaultParams.direction = Eigen::Quaternion<float, 0>::Identity();
- return armarx::skills::SkillDescription(
- armarx::skills::SkillID(constants::skill_names::MoveXMeters),
- "Relative movement in specified direction",
- defaultParams.toAron(),
- armarx::core::time::Duration::Minutes(1),
- ParamType::ToAronType());
+ return armarx::skills::SkillDescription{
+ .skillId = {.skillName = constants::skill_names::MoveXMeters},
+ .description = "Relative movement in specified direction",
+ .rootProfileDefaults = defaultParams.toAron(),
+ .timeout = armarx::core::time::Duration::Minutes(1),
+ .parametersType = ParamType::ToAronType()};
}
};
diff --git a/source/armarx/navigation/skills/NavigateTo.h b/source/armarx/navigation/skills/NavigateTo.h
index d328553a..d71eb317 100644
--- a/source/armarx/navigation/skills/NavigateTo.h
+++ b/source/armarx/navigation/skills/NavigateTo.h
@@ -61,11 +61,11 @@ namespace armarx::navigation::skills
static armarx::skills::SkillDescription
DefaultSkillDescription()
{
- return armarx::skills::SkillDescription({constants::skill_names::NavigateTo},
- "",
- nullptr,
- armarx::core::time::Duration::Days(42),
- Params::ToAronType());
+ return armarx::skills::SkillDescription{
+ .skillId = {.skillName = constants::skill_names::NavigateTo},
+ .description = "",
+ .timeout = armarx::core::time::Duration::Days(42),
+ .parametersType = Params::ToAronType()};
}
protected:
diff --git a/source/armarx/navigation/skills/NavigateToLocation.h b/source/armarx/navigation/skills/NavigateToLocation.h
index 3f76a74d..b05fc964 100644
--- a/source/armarx/navigation/skills/NavigateToLocation.h
+++ b/source/armarx/navigation/skills/NavigateToLocation.h
@@ -66,11 +66,12 @@ namespace armarx::navigation::skills
defaultParameters.location = "";
defaultParameters.velocityLimitAngular = std::nullopt;
defaultParameters.velocityLimitLinear = std::nullopt;
- return armarx::skills::SkillDescription({constants::skill_names::NavigateToLocation},
- "",
- defaultParameters.toAron(),
- armarx::core::time::Duration::Days(42),
- Params::ToAronType());
+ return armarx::skills::SkillDescription{
+ .skillId = {.skillName = constants::skill_names::NavigateToLocation},
+ .description = "",
+ .rootProfileDefaults = defaultParameters.toAron(),
+ .timeout = armarx::core::time::Duration::Days(42),
+ .parametersType = Params::ToAronType()};
}
protected:
diff --git a/source/armarx/navigation/skills/RotateXDegrees.h b/source/armarx/navigation/skills/RotateXDegrees.h
index f874648d..47384ae6 100644
--- a/source/armarx/navigation/skills/RotateXDegrees.h
+++ b/source/armarx/navigation/skills/RotateXDegrees.h
@@ -26,12 +26,12 @@ namespace armarx::navigation::skills
ParamType defaultParams;
defaultParams.rotationDegrees = 0;
- return armarx::skills::SkillDescription(
- {constants::skill_names::RotateXDegrees},
- "Rotate around the Z-axis by a specified degree value",
- defaultParams.toAron(),
- armarx::core::time::Duration::Minutes(1),
- ParamType::ToAronType());
+ return armarx::skills::SkillDescription{
+ .skillId = {.skillName = constants::skill_names::RotateXDegrees},
+ .description = "Rotate around the Z-axis by a specified degree value",
+ .rootProfileDefaults = defaultParams.toAron(),
+ .timeout = armarx::core::time::Duration::Minutes(1),
+ .parametersType = ParamType::ToAronType()};
}
};
--
GitLab