From ad92494538094e047b737c060920650578625ce4 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:36:43 +0200
Subject: [PATCH 01/47] trajectory_control: only robot is necessary (instead of
full scene)
---
.../TrajectoryController.cpp | 6 ++---
.../trajectory_control/TrajectoryController.h | 6 +----
.../TrajectoryFollowingController.cpp | 24 +++++++++----------
.../TrajectoryFollowingController.h | 4 +++-
.../trajectory_control/WaypointController.cpp | 8 +++----
.../trajectory_control/WaypointController.h | 2 +-
.../trajectory_control/aron_conversions.cpp | 4 ++--
.../trajectory_control/aron_conversions.h | 6 +----
8 files changed, 26 insertions(+), 34 deletions(-)
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
index 8252cedb..be79e84e 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
@@ -2,8 +2,8 @@
namespace armarx::navigation::traj_ctrl
{
- TrajectoryController::TrajectoryController(const core::Scene& context) : context(context)
- {
- }
+ // TrajectoryController::TrajectoryController(const core::Scene& context) : context(context)
+ // {
+ // }
} // namespace armarx::navigation::traj_ctrl
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.h b/source/armarx/navigation/trajectory_control/TrajectoryController.h
index f13e168a..72d5927d 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryController.h
+++ b/source/armarx/navigation/trajectory_control/TrajectoryController.h
@@ -61,15 +61,11 @@ namespace armarx::navigation::traj_ctrl
class TrajectoryController
{
public:
- TrajectoryController(const core::Scene& context);
+ TrajectoryController() = default;
virtual ~TrajectoryController() = default;
virtual TrajectoryControllerResult control(const core::Trajectory& goal) = 0;
- protected:
- const core::Scene& context;
-
- private:
};
using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index 1997cb8f..2a68520c 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -34,7 +34,7 @@ namespace armarx::navigation::traj_ctrl
arondto::TrajectoryFollowingControllerParams dto;
TrajectoryFollowingControllerParams bo;
- aron_conv::toAron(dto, bo);
+ armarx::navigation::traj_ctrl::toAron(dto, bo);
return dto.toAron();
}
@@ -46,16 +46,17 @@ namespace armarx::navigation::traj_ctrl
dto.fromAron(dict);
TrajectoryFollowingControllerParams bo;
- aron_conv::fromAron(dto, bo);
+ armarx::navigation::traj_ctrl::fromAron(dto, bo);
return bo;
}
// TrajectoryFollowingController
- TrajectoryFollowingController::TrajectoryFollowingController(const Params& params,
- const core::Scene& context) :
- TrajectoryController(context),
+ TrajectoryFollowingController::TrajectoryFollowingController(
+ const VirtualRobot::RobotPtr& robot,
+ const Params& params) :
+ robot(robot),
params(params),
pidPos(params.pidPos.Kp,
params.pidPos.Ki,
@@ -93,11 +94,8 @@ namespace armarx::navigation::traj_ctrl
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
- };
+ 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());
@@ -128,9 +126,9 @@ namespace armarx::navigation::traj_ctrl
TrajectoryControllerResult
TrajectoryFollowingController::control(const core::Trajectory& trajectory)
{
- ARMARX_CHECK_NOT_NULL(context.robot) << "Robot not available";
+ ARMARX_CHECK_NOT_NULL(robot) << "Robot not available";
- const core::Pose currentPose(context.robot->getGlobalPose());
+ const core::Pose currentPose(robot->getGlobalPose());
const auto projectedPose = trajectory.getProjection(
currentPose.translation(), core::VelocityInterpolation::LinearInterpolation);
@@ -182,7 +180,7 @@ namespace armarx::navigation::traj_ctrl
const auto feedforwardVelocity = desiredMovementDirection * ffVel;
ARMARX_VERBOSE << deactivateSpam(1) << "Feed forward direction "
- << feedforwardVelocity.normalized();
+ << feedforwardVelocity.normalized();
ARMARX_VERBOSE << deactivateSpam(1) << "Feed forward velocity " << feedforwardVelocity;
ARMARX_VERBOSE << deactivateSpam(1) << "Control value " << pidPos.getControlValue();
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
index a9a8a94c..ffb81732 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
@@ -48,7 +48,7 @@ namespace armarx::navigation::traj_ctrl
public:
using Params = TrajectoryFollowingControllerParams;
- TrajectoryFollowingController(const Params& params, const core::Scene& context);
+ TrajectoryFollowingController(const VirtualRobot::RobotPtr& robot, const Params& params);
~TrajectoryFollowingController() override = default;
TrajectoryControllerResult control(const core::Trajectory& trajectory) override;
@@ -57,8 +57,10 @@ namespace armarx::navigation::traj_ctrl
core::Twist applyTwistLimits(core::Twist twist);
private:
+ const VirtualRobot::RobotPtr robot;
const Params params;
+
MultiDimPIDControllerTemplate<3> pidPos;
MultiDimPIDControllerTemplate<3> pidPosTarget;
MultiDimPIDControllerTemplate<3> pidOri;
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.cpp b/source/armarx/navigation/trajectory_control/WaypointController.cpp
index c08ff5b9..527bd4f3 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.cpp
+++ b/source/armarx/navigation/trajectory_control/WaypointController.cpp
@@ -21,7 +21,7 @@ namespace armarx::navigation::traj_ctrl
arondto::WaypointControllerParams dto;
WaypointControllerParams bo;
- aron_conv::toAron(dto, bo);
+ armarx::navigation::traj_ctrl::toAron(dto, bo);
return dto.toAron();
}
@@ -33,15 +33,15 @@ namespace armarx::navigation::traj_ctrl
dto.fromAron(dict);
WaypointControllerParams bo;
- aron_conv::fromAron(dto, bo);
+ armarx::navigation::traj_ctrl::fromAron(dto, bo);
return bo;
}
// WaypointController
- WaypointController::WaypointController(const Params& params, const core::Scene& context) :
- TrajectoryController(context), params(params)
+ WaypointController::WaypointController(const Params& params) :
+ params(params)
{
}
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.h b/source/armarx/navigation/trajectory_control/WaypointController.h
index 13133e67..ec5d4372 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.h
+++ b/source/armarx/navigation/trajectory_control/WaypointController.h
@@ -39,7 +39,7 @@ namespace armarx::navigation::traj_ctrl
public:
using Params = WaypointControllerParams;
- WaypointController(const Params& params, const core::Scene& context);
+ WaypointController(const Params& params);
~WaypointController() override = default;
TrajectoryControllerResult control(const core::Trajectory& goal) override;
diff --git a/source/armarx/navigation/trajectory_control/aron_conversions.cpp b/source/armarx/navigation/trajectory_control/aron_conversions.cpp
index d8d6d898..2cc2d0b4 100644
--- a/source/armarx/navigation/trajectory_control/aron_conversions.cpp
+++ b/source/armarx/navigation/trajectory_control/aron_conversions.cpp
@@ -13,7 +13,7 @@
#include <armarx/navigation/trajectory_control/aron/WaypointControllerParams.aron.generated.h>
-namespace armarx::navigation::traj_ctrl::aron_conv
+namespace armarx::navigation::traj_ctrl
{
void
@@ -67,4 +67,4 @@ namespace armarx::navigation::traj_ctrl::aron_conv
}
-} // namespace armarx::navigation::traj_ctrl::aron_conv
+} // namespace armarx::navigation::traj_ctrl
diff --git a/source/armarx/navigation/trajectory_control/aron_conversions.h b/source/armarx/navigation/trajectory_control/aron_conversions.h
index 14954575..f8496c98 100644
--- a/source/armarx/navigation/trajectory_control/aron_conversions.h
+++ b/source/armarx/navigation/trajectory_control/aron_conversions.h
@@ -39,10 +39,6 @@ namespace armarx::navigation::traj_ctrl
class WaypointControllerParams;
} // namespace arondto
-} // namespace armarx::navigation::traj_ctrl
-
-namespace armarx::navigation::traj_ctrl::aron_conv
-{
void toAron(arondto::TrajectoryControllerParams& dto, const TrajectoryControllerParams& bo);
void fromAron(const arondto::TrajectoryControllerParams& dto, TrajectoryControllerParams& bo);
@@ -55,4 +51,4 @@ namespace armarx::navigation::traj_ctrl::aron_conv
void toAron(arondto::WaypointControllerParams& dto, const WaypointControllerParams& bo);
void fromAron(const arondto::WaypointControllerParams& dto, WaypointControllerParams& bo);
-} // namespace armarx::navigation::traj_ctrl::aron_conv
+} // namespace armarx::navigation::traj_ctrl
--
GitLab
From fdc9aa97be1c910a53c983316a15ce416e74b6d6 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:37:17 +0200
Subject: [PATCH 02/47] scene: no longer needing thread-safe version
---
source/armarx/navigation/core/Trajectory.h | 2 ++
source/armarx/navigation/core/types.h | 4 ----
2 files changed, 2 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/core/Trajectory.h b/source/armarx/navigation/core/Trajectory.h
index e9985faf..fd5f7e4f 100644
--- a/source/armarx/navigation/core/Trajectory.h
+++ b/source/armarx/navigation/core/Trajectory.h
@@ -66,6 +66,8 @@ namespace armarx::navigation::core
class Trajectory
{
public:
+ Trajectory() = default;
+
Trajectory(const std::vector<TrajectoryPoint>& points) : pts(points)
{
}
diff --git a/source/armarx/navigation/core/types.h b/source/armarx/navigation/core/types.h
index a0529cb0..3b5df564 100644
--- a/source/armarx/navigation/core/types.h
+++ b/source/armarx/navigation/core/types.h
@@ -78,18 +78,14 @@ namespace armarx::navigation::core
struct Scene
{
- mutable std::mutex staticSceneMtx;
std::optional<core::StaticScene> staticScene = std::nullopt;
- mutable std::mutex dynamicSceneMtx;
std::optional<core::DynamicScene> dynamicScene = std::nullopt;
// TopologicMapPtr topologicMap;
VirtualRobot::RobotPtr robot;
- mutable std::mutex platformVelocityMtx;
std::optional<core::Twist> platformVelocity;
- mutable std::mutex graphMtx;
std::optional<core::SceneGraph> graph;
TimeServerInterface* timeServer;
--
GitLab
From 12aeec884df68ddee8f88149390c976b90600b32 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:37:41 +0200
Subject: [PATCH 03/47] global_planning: removing mutex
---
source/armarx/navigation/global_planning/AStar.cpp | 1 -
source/armarx/navigation/global_planning/SPFA.cpp | 1 -
2 files changed, 2 deletions(-)
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index fdac1e34..3dcd307c 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -158,7 +158,6 @@ namespace armarx::navigation::global_planning
{
ARMARX_TRACE;
- std::lock_guard g{scene.staticSceneMtx};
// FIXME check if costmap is available
algorithm::astar::AStarPlanner planner(*scene.staticScene->costmap);
diff --git a/source/armarx/navigation/global_planning/SPFA.cpp b/source/armarx/navigation/global_planning/SPFA.cpp
index b5351314..e5c1c7cd 100644
--- a/source/armarx/navigation/global_planning/SPFA.cpp
+++ b/source/armarx/navigation/global_planning/SPFA.cpp
@@ -88,7 +88,6 @@ namespace armarx::navigation::global_planning
{
ARMARX_TRACE;
- std::lock_guard g{scene.staticSceneMtx};
// FIXME check if costmap is available
algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams;
--
GitLab
From 2d5c0739c3fc204e9cf8fd374c03c8788e8cdf40 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:38:03 +0200
Subject: [PATCH 04/47] server/introspection: removing
onTrajectoryControllerResult and onSafetyControllerResult as it is no longer
available
---
.../introspection/ArvizIntrospector.cpp | 32 +++++++++----------
.../server/introspection/ArvizIntrospector.h | 5 +--
.../introspection/IntrospectorInterface.h | 3 --
.../introspection/MemoryIntrospector.cpp | 12 -------
.../server/introspection/MemoryIntrospector.h | 3 --
5 files changed, 17 insertions(+), 38 deletions(-)
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index b6c88e55..4d1dbc1b 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -59,22 +59,22 @@ namespace armarx::navigation::server
drawLocalTrajectory(result.trajectory);
}
- void
- ArvizIntrospector::onTrajectoryControllerResult(
- const traj_ctrl::TrajectoryControllerResult& result)
- {
- std::lock_guard g{mtx};
-
- drawRawVelocity(result.twist);
- }
-
- void
- ArvizIntrospector::onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result)
- {
- std::lock_guard g{mtx};
-
- drawSafeVelocity(result.twist);
- }
+ // void
+ // ArvizIntrospector::onTrajectoryControllerResult(
+ // const traj_ctrl::TrajectoryControllerResult& result)
+ // {
+ // std::lock_guard g{mtx};
+
+ // drawRawVelocity(result.twist);
+ // }
+
+ // void
+ // ArvizIntrospector::onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result)
+ // {
+ // std::lock_guard g{mtx};
+
+ // drawSafeVelocity(result.twist);
+ // }
void
ArvizIntrospector::onGoal(const core::Pose& goal)
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index e91d4b60..a3daddc4 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -54,10 +54,7 @@ namespace armarx::navigation::server
void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) override;
- void
- onTrajectoryControllerResult(const traj_ctrl::TrajectoryControllerResult& result) override;
- void onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result) override;
-
+
void onGoal(const core::Pose& goal) override;
void success();
diff --git a/source/armarx/navigation/server/introspection/IntrospectorInterface.h b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
index 92176601..18eaa36b 100644
--- a/source/armarx/navigation/server/introspection/IntrospectorInterface.h
+++ b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
@@ -44,9 +44,6 @@ namespace armarx::navigation::server
virtual void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) = 0;
virtual void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) = 0;
- virtual void
- onTrajectoryControllerResult(const traj_ctrl::TrajectoryControllerResult& result) = 0;
- virtual void onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result) = 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 226a0c80..aaf84a09 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
@@ -22,18 +22,6 @@ namespace armarx::navigation::server
// TODO(fabian.reister): implement
}
- void
- MemoryIntrospector::onTrajectoryControllerResult(
- const traj_ctrl::TrajectoryControllerResult& result)
- {
- // TODO(fabian.reister): implement
- }
-
- void
- MemoryIntrospector::onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result)
- {
- // TODO(fabian.reister): implement
- }
void
MemoryIntrospector::onGoal(const core::Pose& goal)
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.h b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
index a141d4ba..9d473a74 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.h
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
@@ -40,9 +40,6 @@ namespace armarx::navigation::server
void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) override;
- void
- onTrajectoryControllerResult(const traj_ctrl::TrajectoryControllerResult& result) override;
- void onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result) override;
void onGoal(const core::Pose& goal) override;
--
GitLab
From e10793f03da3cd9a7c6c157bc8d210dc56014922 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:39:34 +0200
Subject: [PATCH 05/47] server/execution: using RT-Controller instead
---
.../server/execution/ExecutorInterface.h | 5 +-
.../execution/PlatformControllerExecutor.cpp | 36 ++++++++++++++
.../execution/PlatformControllerExecutor.h | 48 +++++++++++++++++++
3 files changed, 87 insertions(+), 2 deletions(-)
create mode 100644 source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
create mode 100644 source/armarx/navigation/server/execution/PlatformControllerExecutor.h
diff --git a/source/armarx/navigation/server/execution/ExecutorInterface.h b/source/armarx/navigation/server/execution/ExecutorInterface.h
index e0ba451e..c4a38f29 100644
--- a/source/armarx/navigation/server/execution/ExecutorInterface.h
+++ b/source/armarx/navigation/server/execution/ExecutorInterface.h
@@ -2,7 +2,7 @@
namespace armarx::navigation::core
{
- struct Twist;
+ class Trajectory;
} // namespace armarx::navigation::core
namespace armarx::navigation::server
@@ -17,7 +17,8 @@ namespace armarx::navigation::server
public:
virtual ~ExecutorInterface() = default;
- virtual void move(const core::Twist& twist) = 0;
+ virtual void execute(const core::Trajectory& trajectory) = 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
new file mode 100644
index 00000000..90acc53b
--- /dev/null
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -0,0 +1,36 @@
+#include "PlatformControllerExecutor.h"
+
+#include <armarx/control/client/ComponentPlugin.h>
+#include <armarx/control/common/type.h>
+#include <armarx/navigation/common/controller_types.h>
+#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+
+
+
+namespace armarx::navigation::server
+{
+
+ PlatformControllerExecutor::PlatformControllerExecutor(
+ ControllerComponentPlugin& controllerComponentPlugin) :
+ controllerPlugin(controllerComponentPlugin)
+ {
+ controllerComponentPlugin.getRobotUnitPlugin().getRobotUnit()->loadLibFromPackage(
+ "armarx_navigation", "libarmarx_navigation_platform_controller.so");
+
+
+ auto builder = controllerPlugin.createControllerBuilder<
+ armarx::navigation::common::ControllerType::PlatformTrajectory>();
+
+ ctrl.emplace(builder.create().value());
+
+ ARMARX_CHECK(ctrl.has_value());
+ }
+
+ PlatformControllerExecutor::~PlatformControllerExecutor() = default;
+
+
+ void
+ PlatformControllerExecutor::execute(const core::Trajectory& trajectory)
+ {
+ }
+} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
new file mode 100644
index 00000000..1b2ec239
--- /dev/null
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#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>
+
+namespace armarx::control::client
+{
+ class ComponentPlugin;
+}
+
+namespace armarx::navigation::server
+{
+
+ /**
+ * @brief The PlatformUnitExecutor class
+ *
+ * TODO: Should be renamed to whatever the new unit will be called which takes Mat4f
+ * as input instead of X/Y/Yaw, so that we have a generic interface.
+ */
+ class PlatformControllerExecutor : virtual public ExecutorInterface
+ {
+
+ public:
+ using ControllerComponentPlugin = armarx::control::client::ComponentPlugin;
+
+ PlatformControllerExecutor(ControllerComponentPlugin& controllerComponentPlugin);
+ ~PlatformControllerExecutor() override;
+
+ void execute(const core::Trajectory& trajectory) override;
+ void stop() override{
+ // FIXME implement
+ };
+
+ private:
+ std::optional<armarx::control::client::ControllerWrapper<
+ armarx::navigation::common::ControllerType::PlatformTrajectory>>
+ ctrl;
+
+ ControllerComponentPlugin& controllerPlugin;
+ };
+
+} // namespace armarx::navigation::server
--
GitLab
From ba3ca6872af184daa946aa63ad4be15de15e1801 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:40:15 +0200
Subject: [PATCH 06/47] server/monitoring: removing mutex
---
.../navigation/server/monitoring/GoalReachedMonitor.cpp | 4 ----
1 file changed, 4 deletions(-)
diff --git a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp
index bb7e134b..923eeb12 100644
--- a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp
+++ b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp
@@ -42,8 +42,6 @@ namespace armarx::navigation::server
ARMARX_DEBUG << "Orientation goal reached";
}
- std::unique_lock lock{
- scene.platformVelocityMtx}; // thread-safe access to scene.platformVelocity
if (not scene.platformVelocity.has_value())
{
@@ -55,8 +53,6 @@ namespace armarx::navigation::server
const bool angularVelocityLow =
scene.platformVelocity->angular.norm() < config.angularVelTh;
- lock.unlock();
-
const bool velocityLow = linearVelocityLow and angularVelocityLow;
if (velocityLow)
--
GitLab
From ac4da72af93f0e129203764770bb13a236f21501 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:40:56 +0200
Subject: [PATCH 07/47] server/Navigator: simplification regarding threading
---
.../navigation/server/NavigationStack.h | 4 +-
source/armarx/navigation/server/Navigator.cpp | 225 ++++--------------
source/armarx/navigation/server/Navigator.h | 31 +--
3 files changed, 53 insertions(+), 207 deletions(-)
diff --git a/source/armarx/navigation/server/NavigationStack.h b/source/armarx/navigation/server/NavigationStack.h
index 9c047123..c2a76f9a 100644
--- a/source/armarx/navigation/server/NavigationStack.h
+++ b/source/armarx/navigation/server/NavigationStack.h
@@ -34,7 +34,7 @@ namespace armarx::navigation::server
{
global_planning::GlobalPlannerPtr globalPlanner;
loc_plan::LocalPlannerPtr localPlanner = nullptr;
- traj_ctrl::TrajectoryControllerPtr trajectoryControl;
- safe_ctrl::SafetyControllerPtr safetyControl = nullptr;
+ // traj_ctrl::TrajectoryControllerPtr trajectoryControl;
+ // safe_ctrl::SafetyControllerPtr safetyControl = nullptr;
};
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 3a1b42f2..bf343cba 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -624,62 +624,18 @@ namespace armarx::navigation::server
ARMARX_TRACE;
ARMARX_INFO << "Planning local trajectory enabled";
- localPlannerTask =
+ replanningTask =
new PeriodicTask<Navigator>(this,
&Navigator::updateLocalPlanner,
- config.general.tasks.localPlanningUpdatePeriod,
+ config.general.tasks.replanningUpdatePeriod,
false,
"LocalPlannerTask");
- localPlannerTask->start();
+ replanningTask->start();
}
- // trajectory controller
- ARMARX_CHECK_NOT_NULL(config.stack.trajectoryControl);
- trajectoryControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateTrajectoryController,
- config.general.tasks.trajectoryControllerUpdatePeriod,
- false,
- "TrajectoryControllerTask");
- trajectoryControllerTask->start();
-
- // safety controller
- if (config.stack.safetyControl)
- {
- ARMARX_INFO << "Safety control enabled";
- safetyControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateSafetyController,
- config.general.tasks.safetyControllerUpdatePeriod,
- false,
- "SafetyControllerTask");
- safetyControllerTask->start();
- }
-
- // executor
- executorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateExecutor,
- config.general.tasks.executorUpdatePeriod,
- false,
- "ExecutorTask");
- executorTask->start();
-
- // introspector
- introspectorTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateIntrospector,
- config.general.tasks.introspectorUpdatePeriod,
- false,
- "IntrospectorTask");
- introspectorTask->start();
// monitoring
ARMARX_INFO << "Starting monitoring";
- // std::vector<core::Pose> waypoints = globalPlan->trajectory.poses();
-
-
- // goalReachedMonitor =
- // GoalReachedMonitor(waypoints.back(), *config.scene, GoalReachedMonitorConfig());
monitorTask = new PeriodicTask<Navigator>(this,
&Navigator::updateMonitor,
config.general.tasks.monitorUpdatePeriod,
@@ -750,7 +706,7 @@ namespace armarx::navigation::server
ARMARX_TRACE;
srv.publisher->globalTrajectoryUpdated(globalPlan.value());
- // srv.introspector->onGlobalPlan(globalPlan.value());
+ srv.introspector->onGlobalPlannerResult(globalPlan.value());
ARMARX_INFO << "Global planning completed. Will now start all required threads";
ARMARX_TRACE;
@@ -759,58 +715,17 @@ namespace armarx::navigation::server
if (config.stack.localPlanner)
{
ARMARX_INFO << "Planning local trajectory enabled";
- localPlannerTask =
+ replanningTask =
new PeriodicTask<Navigator>(this,
- &Navigator::updateLocalPlanner,
- config.general.tasks.localPlanningUpdatePeriod,
+ &Navigator::replan,
+ config.general.tasks.replanningUpdatePeriod,
false,
"LocalPlannerTask");
- localPlannerTask->start();
+ replanningTask->start();
}
- // trajectory controller
- ARMARX_CHECK_NOT_NULL(config.stack.trajectoryControl);
- trajectoryControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateTrajectoryController,
- config.general.tasks.trajectoryControllerUpdatePeriod,
- false,
- "TrajectoryControllerTask");
- trajectoryControllerTask->start();
-
- // safety controller
- if (config.stack.safetyControl)
- {
- ARMARX_INFO << "Safety control enabled";
- safetyControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateSafetyController,
- config.general.tasks.safetyControllerUpdatePeriod,
- false,
- "SafetyControllerTask");
- safetyControllerTask->start();
- }
-
- // executor
- executorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateExecutor,
- config.general.tasks.executorUpdatePeriod,
- false,
- "ExecutorTask");
- executorTask->start();
-
- // introspector
- introspectorTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateIntrospector,
- config.general.tasks.introspectorUpdatePeriod,
- false,
- "IntrospectorTask");
- introspectorTask->start();
-
// monitoring
ARMARX_INFO << "Starting monitoring";
-
monitorTask = new PeriodicTask<Navigator>(this,
&Navigator::updateMonitor,
config.general.tasks.monitorUpdatePeriod,
@@ -841,6 +756,14 @@ namespace armarx::navigation::server
{
}
+ void
+ Navigator::replan()
+ {
+ updateLocalPlanner();
+ updateExecutor();
+ updateIntrospector();
+ }
+
void
Navigator::updateLocalPlanner()
{
@@ -856,31 +779,6 @@ namespace armarx::navigation::server
}
}
- void
- Navigator::updateTrajectoryController()
- {
- // TODO distinguish between global and local plan!
-
- if (not globalPlan.has_value())
- {
- return;
- }
-
- trajectoryCtrlResult = config.stack.trajectoryControl->control(globalPlan->trajectory);
-
- srv.publisher->trajectoryControllerUpdated(trajectoryCtrlResult.value());
- }
-
- void
- Navigator::updateSafetyController()
- {
- // FIXME
- // if (res.controlVelocity)
- // {
- // res.safeVelocity = config.stack.safetyControl->control(res.controlVelocity.value());
- // }
- }
-
void
Navigator::updateExecutor()
{
@@ -898,32 +796,23 @@ namespace armarx::navigation::server
return;
}
- const core::Twist twist = [&]() -> core::Twist
- {
- if (config.stack.safetyControl != nullptr)
- {
- return safetyCtrlResult->twist;
- }
- return trajectoryCtrlResult->twist;
- }();
+ // const core::Rotation robot_R_world(config.scene->robot->getGlobalOrientation().inverse());
- const core::Rotation robot_R_world(config.scene->robot->getGlobalOrientation().inverse());
+ // ARMARX_VERBOSE
+ // << deactivateSpam(100) << "Robot orientation "
+ // << simox::math::mat3f_to_rpy(config.scene->robot->getGlobalOrientation()).z();
- ARMARX_VERBOSE
- << deactivateSpam(100) << "Robot orientation "
- << simox::math::mat3f_to_rpy(config.scene->robot->getGlobalOrientation()).z();
+ // core::Twist robotFrameVelocity;
- core::Twist robotFrameVelocity;
+ // robotFrameVelocity.linear = robot_R_world * twist.linear;
+ // // FIXME fix angular velocity
+ // robotFrameVelocity.angular = twist.angular;
- robotFrameVelocity.linear = robot_R_world * twist.linear;
- // FIXME fix angular velocity
- robotFrameVelocity.angular = twist.angular;
+ // ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
+ // << robotFrameVelocity.linear;
- ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
- << robotFrameVelocity.linear;
-
- srv.executor->move(robotFrameVelocity);
+ srv.executor->execute(currentTrajectory());
}
void
@@ -932,30 +821,16 @@ namespace armarx::navigation::server
ARMARX_CHECK_NOT_NULL(srv.introspector);
// TODO event driven. should be committed only on change
- if(globalPlan.has_value())
- {
- srv.introspector->onGlobalPlannerResult(globalPlan.value());
- }
+ // if(globalPlan.has_value())
+ // {
+ // srv.introspector->onGlobalPlannerResult(globalPlan.value());
+ // }
if (localPlan.has_value())
{
srv.introspector->onLocalPlannerResult(localPlan.value());
}
- if (trajectoryCtrlResult.has_value())
- {
- srv.introspector->onTrajectoryControllerResult(trajectoryCtrlResult.value());
- }
-
- if (safetyCtrlResult.has_value())
- {
- srv.introspector->onSafetyControllerResult(safetyCtrlResult.value());
- }
-
- // if (res.isValid())
- // {
- // srv.introspector->onStackResult(res);
- // }
}
void
@@ -991,11 +866,7 @@ namespace armarx::navigation::server
void
Navigator::stopAllThreads()
{
- stopIfRunning(localPlannerTask);
- stopIfRunning(trajectoryControllerTask);
- stopIfRunning(safetyControllerTask);
- stopIfRunning(executorTask);
- stopIfRunning(introspectorTask);
+ stopIfRunning(replanningTask);
stopIfRunning(monitorTask);
}
@@ -1023,29 +894,21 @@ namespace armarx::navigation::server
}
}
- // trajectory controller
- if (config.stack.trajectoryControl != nullptr)
- {
- if (not trajectoryCtrlResult.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Raw control velocity not yet set.";
- return false;
- }
- }
+ // [[likely]]
+ return true;
+ }
- // safety controller
- if (config.stack.safetyControl != nullptr)
+ const core::Trajectory& Navigator::currentTrajectory() const
+ {
+ ARMARX_CHECK(isStackResultValid());
+
+ if(localPlan.has_value())
{
- if (not safetyCtrlResult.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Safe velocity not yet set.";
- return false;
- }
+ return localPlan->trajectory;
}
- // [[likely]]
- return true;
- }
+ return globalPlan->trajectory;
+ }
void
Navigator::pause()
@@ -1055,7 +918,7 @@ namespace armarx::navigation::server
executorEnabled.store(false);
ARMARX_CHECK_NOT_NULL(srv.executor);
- srv.executor->move(core::Twist::Zero());
+ srv.executor->stop();
}
void
diff --git a/source/armarx/navigation/server/Navigator.h b/source/armarx/navigation/server/Navigator.h
index a1ccfa3e..933a1c24 100644
--- a/source/armarx/navigation/server/Navigator.h
+++ b/source/armarx/navigation/server/Navigator.h
@@ -62,13 +62,8 @@ namespace armarx::navigation::server
{
struct
{
- // TODO: Use chrono?
- int localPlanningUpdatePeriod{100}; // [ms]
- int trajectoryControllerUpdatePeriod{100}; // [ms]
- int safetyControllerUpdatePeriod{100}; // [ms]
- int introspectorUpdatePeriod{100}; // [ms]
- int executorUpdatePeriod{100}; // [ms]
- int monitorUpdatePeriod{20}; // [ms]
+ int replanningUpdatePeriod{10}; // [ms]
+ int monitorUpdatePeriod{10}; // [ms]
} tasks;
};
@@ -84,7 +79,6 @@ namespace armarx::navigation::server
IntrospectorInterface* introspector = nullptr;
};
- public:
Navigator(const Config& config, const InjectedServices& services);
void moveTo(const std::vector<core::Pose>& waypoints,
@@ -117,15 +111,15 @@ namespace armarx::navigation::server
void moveToAbsolute(const std::vector<core::Pose>& waypoints);
void moveTowardsAbsolute(const core::Direction& direction);
- void updateLocalPlanner();
- void updateTrajectoryController();
- void updateSafetyController();
+ void replan();
+ void updateLocalPlanner();
void updateExecutor();
-
void updateIntrospector();
+
void updateMonitor();
+ const core::Trajectory& currentTrajectory() const;
bool isStackResultValid() const noexcept;
void stopAllThreads();
@@ -143,16 +137,7 @@ namespace armarx::navigation::server
std::atomic_bool executorEnabled = true;
- // StackResult res;
-
- PeriodicTask<Navigator>::pointer_type localPlannerTask;
- PeriodicTask<Navigator>::pointer_type trajectoryControllerTask;
- PeriodicTask<Navigator>::pointer_type safetyControllerTask;
-
- PeriodicTask<Navigator>::pointer_type executorTask;
-
- PeriodicTask<Navigator>::pointer_type introspectorTask;
-
+ PeriodicTask<Navigator>::pointer_type replanningTask;
PeriodicTask<Navigator>::pointer_type monitorTask;
std::optional<GoalReachedMonitor> goalReachedMonitor;
@@ -160,8 +145,6 @@ namespace armarx::navigation::server
std::optional<global_planning::GlobalPlannerResult> globalPlan;
std::optional<loc_plan::LocalPlannerResult> localPlan;
- std::optional<traj_ctrl::TrajectoryControllerResult> trajectoryCtrlResult;
- std::optional<safe_ctrl::SafetyControllerResult> safetyCtrlResult;
};
} // namespace armarx::navigation::server
--
GitLab
From f0b99a7f8e5af2bbf033358f41b5d9b37385e33b Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:41:33 +0200
Subject: [PATCH 08/47] factories: removing .trajectoryControl and
.safetyControl
---
.../armarx/navigation/factories/NavigationStackFactory.cpp | 6 +++---
.../navigation/factories/TrajectoryControllerFactory.cpp | 7 ++++---
.../navigation/factories/TrajectoryControllerFactory.h | 5 +++--
3 files changed, 10 insertions(+), 8 deletions(-)
diff --git a/source/armarx/navigation/factories/NavigationStackFactory.cpp b/source/armarx/navigation/factories/NavigationStackFactory.cpp
index 68fa77a0..9869c6a7 100644
--- a/source/armarx/navigation/factories/NavigationStackFactory.cpp
+++ b/source/armarx/navigation/factories/NavigationStackFactory.cpp
@@ -40,9 +40,9 @@ namespace armarx::navigation::fac
return server::NavigationStack{
.globalPlanner = GlobalPlannerFactory::create(globalPlannerCfg, ctx),
- .localPlanner = LocalPlannerFactory::create(localPlannerCfg, ctx),
- .trajectoryControl = TrajectoryControllerFactory::create(trajectoryControllerCfg, ctx),
- .safetyControl = SafetyControllerFactory::create(safetyControllerCfg, ctx)};
+ .localPlanner = LocalPlannerFactory::create(localPlannerCfg, ctx)};
+ // .trajectoryControl = TrajectoryControllerFactory::create(trajectoryControllerCfg, ctx),
+ // .safetyControl = SafetyControllerFactory::create(safetyControllerCfg, ctx)};
}
server::NavigationStack
diff --git a/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp b/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
index 277d277d..3b8f6a39 100644
--- a/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
+++ b/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
@@ -12,7 +12,8 @@
namespace armarx::navigation::fac
{
traj_ctrl::TrajectoryControllerPtr
- TrajectoryControllerFactory::create(const aron::data::DictPtr& params, const core::Scene& ctx)
+ TrajectoryControllerFactory::create(const VirtualRobot::RobotPtr& robot,
+ const aron::data::DictPtr& params)
{
namespace layer = traj_ctrl;
@@ -35,11 +36,11 @@ namespace armarx::navigation::fac
{
case traj_ctrl::Algorithms::WaypointController:
controller = std::make_shared<traj_ctrl::WaypointController>(
- traj_ctrl::WaypointControllerParams::FromAron(algoParams), ctx);
+ traj_ctrl::WaypointControllerParams::FromAron(algoParams));
break;
case traj_ctrl::Algorithms::TrajectoryFollowingController:
controller = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
- traj_ctrl::TrajectoryFollowingControllerParams::FromAron(algoParams), ctx);
+ robot, traj_ctrl::TrajectoryFollowingControllerParams::FromAron(algoParams));
break;
}
diff --git a/source/armarx/navigation/factories/TrajectoryControllerFactory.h b/source/armarx/navigation/factories/TrajectoryControllerFactory.h
index 16328e1c..cd35d860 100644
--- a/source/armarx/navigation/factories/TrajectoryControllerFactory.h
+++ b/source/armarx/navigation/factories/TrajectoryControllerFactory.h
@@ -23,6 +23,7 @@
#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>
@@ -38,8 +39,8 @@ namespace armarx::navigation::fac
class TrajectoryControllerFactory
{
public:
- static traj_ctrl::TrajectoryControllerPtr create(const aron::data::DictPtr& params,
- const core::Scene& ctx);
+ static traj_ctrl::TrajectoryControllerPtr create(const VirtualRobot::RobotPtr& robot,
+ const aron::data::DictPtr& params);
protected:
private:
--
GitLab
From 97ccef228b4dafd107dd650f5f7e83e431a76ac7 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:42:14 +0200
Subject: [PATCH 09/47] common: controller types
---
.../navigation/common/controller_types.h | 18 ++++++++++++++++++
1 file changed, 18 insertions(+)
create mode 100644 source/armarx/navigation/common/controller_types.h
diff --git a/source/armarx/navigation/common/controller_types.h b/source/armarx/navigation/common/controller_types.h
new file mode 100644
index 00000000..905ea6de
--- /dev/null
+++ b/source/armarx/navigation/common/controller_types.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+namespace armarx::navigation::common
+{
+
+ enum class ControllerType
+ {
+ PlatformTrajectory
+ };
+
+ constexpr const char* PlatformTrajectoryControllerName= "PlatformTrajectory";
+
+ inline const simox::meta::EnumNames<ControllerType> ControllerTypeNames{
+ {ControllerType::PlatformTrajectory, PlatformTrajectoryControllerName}};
+
+} // namespace armarx::navigation::common
--
GitLab
From 4dc51162c712ac2dbfcecfd9b465d865e6ba38d9 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:42:29 +0200
Subject: [PATCH 10/47] platform_controller: adding RT controller
---
.../platform_controller/CMakeLists.txt | 24 +++
.../PlatformTrajectoryController.cpp | 99 ++++++++++++
.../PlatformTrajectoryController.h | 152 ++++++++++++++++++
.../PlatformTrajectoryControllerConfig.xml | 43 +++++
.../platform_controller/aron_conversions.cpp | 50 ++++++
.../platform_controller/aron_conversions.h | 35 ++++
.../controller_descriptions.h | 42 +++++
7 files changed, 445 insertions(+)
create mode 100644 source/armarx/navigation/platform_controller/CMakeLists.txt
create mode 100644 source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
create mode 100644 source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
create mode 100644 source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
create mode 100644 source/armarx/navigation/platform_controller/aron_conversions.cpp
create mode 100644 source/armarx/navigation/platform_controller/aron_conversions.h
create mode 100644 source/armarx/navigation/platform_controller/controller_descriptions.h
diff --git a/source/armarx/navigation/platform_controller/CMakeLists.txt b/source/armarx/navigation/platform_controller/CMakeLists.txt
new file mode 100644
index 00000000..abeeec07
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/CMakeLists.txt
@@ -0,0 +1,24 @@
+armarx_add_aron_library(platform_controller_aron
+ ARON_FILES
+ aron/PlatformTrajectoryControllerConfig.xml
+ DEPENDENCIES
+ armarx_control::common_aron
+)
+
+armarx_add_library(platform_controller
+ SOURCES
+ # WholeBodyImpedanceController.cpp
+ PlatformTrajectoryController.cpp
+ aron_conversions.cpp
+ HEADERS
+ # WholeBodyImpedanceController.h
+ PlatformTrajectoryController.h
+ aron_conversions.h
+ DEPENDENCIES_PUBLIC
+ Simox::VirtualRobot
+ armarx_control::common
+ armarx_control::njoint_qp_controller_ice
+ 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/PlatformTrajectoryController.cpp
new file mode 100644
index 00000000..01e7492d
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
@@ -0,0 +1,99 @@
+#include "PlatformTrajectoryController.h"
+
+#include "RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
+
+#include "armarx/navigation/common/controller_types.h"
+#include "armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h"
+#include "armarx/navigation/platform_controller/aron_conversions.h"
+#include <armarx/control/common/aron_conversions.h>
+#include <armarx/control/common/type.h>
+#include <armarx/control/common/utils.h>
+
+namespace armarx::navigation::platform_controller::platform_trajectory
+{
+ Controller::Controller(const RobotUnitPtr& robotUnit,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&)
+ {
+
+ // 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::PlatformTrajectory);
+ }
+
+ void
+ Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+ }
+
+ 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_IMPORTANT << "done Controller::updateConfig";
+ }
+
+ void
+ Controller::additionalTask()
+ {
+ }
+
+ void
+ Controller::onPublish(const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& debugDrawer,
+ const DebugObserverInterfacePrx& debugObservers)
+ {
+ }
+
+ void
+ Controller::onInitNJointController()
+ {
+ }
+
+ void
+ Controller::rtPreActivateController()
+ {
+ }
+
+
+ Controller::~Controller() = default;
+
+} // namespace armarx::navigation::platform_controller::platform_trajectory
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
new file mode 100644
index 00000000..a4277f67
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.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/>.
+ *
+ * @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/navigation/trajectory_control/TrajectoryFollowingController.h"
+#include <armarx/control/common/control_law/ControllerCommonInterface.h>
+#include <armarx/control/njoint_controller/task_space/ControllerInterface.h>
+#include <armarx/control/njoint_qp_controller/ControllerInterface.h>
+
+#include <armarx/navigation/core/types.h>
+
+namespace armarx
+{
+ class ControlTargetHolonomicPlatformVelocity;
+ class SensorValueHolonomicPlatformWithAbsolutePosition;
+} // namespace armarx
+
+
+namespace armarx::navigation::platform_controller::platform_trajectory
+{
+ struct Targets
+ {
+ core::Trajectory trajectory;
+ };
+
+ struct Config
+ {
+ using Params = traj_ctrl::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 = NJointTaskspaceControllerConfigPtr;
+
+ 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:
+ // [[nodiscard]] bool initializeQPIK();
+
+ // void updateDebugStatus();
+
+ // struct DebugStatus
+ // {
+ // std::map<std::string, float> data;
+ // };
+
+ TripleBuffer<Config> configBuffer;
+
+ // internal
+ std::atomic_bool rtFirstRun = true;
+ std::atomic_bool rtReady = false;
+
+ ControlTargetHolonomicPlatformVelocity* platformTarget;
+
+ std::optional<traj_ctrl::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::control::platform_controller::platform_trajectory
diff --git a/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml b/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
new file mode 100644
index 00000000..a6551b53
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
@@ -0,0 +1,43 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ <Include include="../../core/aron/TwistLimits.xml" />
+ <Include include="../../core/aron/PIDParams.xml" />
+ <Include include="../../core/aron/Trajectory.xml" />
+ <Include include="../../trajectory_control/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'>
+ <ObjectChild key='twistLimits'>
+ <armarx::navigation::core::arondto::TwistLimits />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Targets'>
+ <ObjectChild key='trajectory'>
+ <armarx::navigation::core::arondto::Trajectory />
+ </ObjectChild>
+ </Object>
+
+
+ <Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Config'>
+ <ObjectChild key='params'>
+ <armarx::navigation::traj_ctrl::arondto::TrajectoryFollowingControllerParams />
+ </ObjectChild>
+
+ <ObjectChild key='targets'>
+ <armarx::navigation::platform_controller::platform_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
new file mode 100644
index 00000000..0f32b69e
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/aron_conversions.cpp
@@ -0,0 +1,50 @@
+/**
+ * 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 "aron_conversions.h"
+
+#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>
+
+
+
+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 armarx::navigation::platform_controller::platform_trajectory
diff --git a/source/armarx/navigation/platform_controller/aron_conversions.h b/source/armarx/navigation/platform_controller/aron_conversions.h
new file mode 100644
index 00000000..bd5518b1
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/aron_conversions.h
@@ -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/>.
+ *
+ * @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::navigation::platform_controller::platform_trajectory
+{
+ namespace arondto
+ {
+ class Config;
+ }
+
+ struct Config;
+
+ void fromAron(const arondto::Config& dto, Config& bo);
+} // namespace armarx::navigation::platform_controller::platform_trajectory
diff --git a/source/armarx/navigation/platform_controller/controller_descriptions.h b/source/armarx/navigation/platform_controller/controller_descriptions.h
new file mode 100644
index 00000000..4fa54a75
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/controller_descriptions.h
@@ -0,0 +1,42 @@
+/**
+ * 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/control/client/ControllerDescription.h>
+
+#include <armarx/navigation/common/controller_types.h>
+#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+
+namespace armarx::control::client
+{
+ template <>
+ struct ControllerDescription<armarx::navigation::common::ControllerType::PlatformTrajectory>
+ {
+ using AronDTO =
+ armarx::navigation::platform_controller::platform_trajectory::arondto::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;
+ };
+
+} // namespace armarx::control::client
--
GitLab
From 03eea3fab105a3915ca9be342e3f27877782f0a7 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:42:47 +0200
Subject: [PATCH 11/47] cmake: removing PlatformUnitExecutor
---
source/armarx/navigation/server/CMakeLists.txt | 8 ++++++--
1 file changed, 6 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/server/CMakeLists.txt b/source/armarx/navigation/server/CMakeLists.txt
index 25eae227..a6ec351b 100644
--- a/source/armarx/navigation/server/CMakeLists.txt
+++ b/source/armarx/navigation/server/CMakeLists.txt
@@ -4,7 +4,8 @@ armarx_add_library(server
./StackResult.cpp
./GraphBuilder.cpp
# Executors.
- ./execution/PlatformUnitExecutor.cpp
+ # ./execution/PlatformUnitExecutor.cpp
+ ./execution/PlatformControllerExecutor.cpp
# Event publishing.
./event_publishing/MemoryPublisher.cpp
# Introspection
@@ -21,7 +22,8 @@ armarx_add_library(server
./GraphBuilder.h
# Executors.
./execution/ExecutorInterface.h
- ./execution/PlatformUnitExecutor.h
+ # ./execution/PlatformUnitExecutor.h
+ ./execution/PlatformControllerExecutor.h
./execution/DummyExecutor.h
# Event publishing.
./event_publishing/EventPublishingInterface.h
@@ -45,6 +47,8 @@ armarx_add_library(server
armarx_navigation::trajectory_control
armarx_navigation::safety_control
armarx_navigation::memory
+ armarx_control::client
+ armarx_control::njoint_qp_controller_ice
DEPENDENCIES_PRIVATE
range-v3::range-v3
)
--
GitLab
From e5dabf4a74ed0b15335b3091e2e310ee8bf67f40 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:43:13 +0200
Subject: [PATCH 12/47] components: Navigator simplification
---
.../components/Navigator/Navigator.cpp | 35 +++++++------------
.../components/Navigator/Navigator.h | 19 +++++-----
.../components/Navigator/RemoteGui.cpp | 1 -
3 files changed, 21 insertions(+), 34 deletions(-)
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 7090cb6e..5c4f0337 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -58,6 +58,7 @@
#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+#include "RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
@@ -84,7 +85,6 @@
#include <armarx/navigation/factories/NavigationStackFactory.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
-#include <armarx/navigation/server/execution/PlatformUnitExecutor.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
#include <armarx/navigation/util/util.h>
@@ -117,7 +117,7 @@ namespace armarx::navigation::components
components::Navigator::Navigator() : parameterizationService(nullptr, nullptr)
// publisher(&resultsWriter, &eventsWriter)
{
- scene.timeServer = &timeServer;
+ // scene.timeServer = &timeServer;
addPlugin(parameterizationReaderPlugin);
addPlugin(parameterizationWriterPlugin);
@@ -163,7 +163,7 @@ namespace armarx::navigation::components
ARMARX_TRACE;
initializeScene();
- executor = server::PlatformUnitExecutor(platformUnit);
+ executor.emplace(server::PlatformControllerExecutor(getControlComponentPlugin()));
introspector = server::ArvizIntrospector(getArvizClient(), scene.robot);
@@ -172,12 +172,12 @@ namespace armarx::navigation::components
// TODO param (10)
- robotStateUpdateTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateRobot,
- params.robotUpdatePeriodMs,
- false,
- "RobotStateUpdateTask");
- robotStateUpdateTask->start();
+ // robotStateUpdateTask = new PeriodicTask<Navigator>(this,
+ // &Navigator::updateRobot,
+ // params.robotUpdatePeriodMs,
+ // false,
+ // "RobotStateUpdateTask");
+ // robotStateUpdateTask->start();
navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
navRemoteGui->enable();
@@ -222,7 +222,7 @@ namespace armarx::navigation::components
{
ARMARX_TRACE;
auto robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(
- params.robotName,
+ getControlComponentPlugin().getRobotUnitPlugin().getRobotUnit()->getKinematicUnit()->getRobotName(),
armarx::core::time::Clock::Now(),
VirtualRobot::RobotIO::RobotDescription::eCollisionModel,
true);
@@ -239,8 +239,6 @@ namespace armarx::navigation::components
scene.robot = getRobot();
{
- std::lock_guard g{scene.staticSceneMtx};
-
if (not scene.staticScene.has_value())
{
scene.staticScene = staticScene();
@@ -253,7 +251,6 @@ namespace armarx::navigation::components
// TODO dynamic scene
{
- std::lock_guard g{scene.graphMtx};
scene.graph = core::SceneGraph{.subgraphs = graphReaderPlugin->get().graphs()};
}
}
@@ -392,7 +389,7 @@ namespace armarx::navigation::components
navigators.at(configId).stop();
// emit UserAbortTriggered event
- const core::UserAbortTriggeredEvent event{{scene.timeServer->now()},
+ const core::UserAbortTriggeredEvent event{{timeServer.now()},
core::Pose(scene.robot->getGlobalPose())};
memoryPublishers.at(configId)->userAbortTriggered(event);
}
@@ -409,7 +406,7 @@ namespace armarx::navigation::components
}
// emit UserAbortTriggered event
- const core::UserAbortTriggeredEvent event{{scene.timeServer->now()},
+ const core::UserAbortTriggeredEvent event{{timeServer.now()},
core::Pose(scene.robot->getGlobalPose())};
for (auto& [callerId, memoryPublisher] : memoryPublishers)
{
@@ -449,14 +446,10 @@ namespace armarx::navigation::components
// def->topic<PlatformUnitListener>("MyTopic");
// Use (and depend on) another component (passing the ComponentInterfacePrx).
- def->component(platformUnit);
-
def->component(remoteGui, "RemoteGuiProvider");
// Add a required property.
// def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
- def->optional(params.robotName, "p.robotName");
-
// Add an optionalproperty.
def->optional(params.occupiedGridThreshold,
"p.occupancy_grid.occopied_threshold",
@@ -640,7 +633,7 @@ namespace armarx::navigation::components
ARMARX_TRACE;
const auto robotDescription =
- virtualRobotReader.queryDescription(params.robotName, timestamp);
+ virtualRobotReader.queryDescription(scene.robot->getType(), timestamp);
ARMARX_CHECK(robotDescription.has_value());
// synchronizing platform state
@@ -651,8 +644,6 @@ namespace armarx::navigation::components
// TODO / FIXME make this thread safe!
{
- std::lock_guard g{scene.platformVelocityMtx};
-
if (not scene.platformVelocity.has_value())
{
scene.platformVelocity = core::Twist();
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 6fa6459d..618b378d 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -33,14 +33,15 @@
#include <Ice/Object.h>
+#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include "RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h"
#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
-#include <RobotAPI/interface/units/PlatformUnitInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
@@ -50,6 +51,7 @@
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include "armarx/navigation/memory/client/costmap/Writer.h"
+#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/components/Navigator/RemoteGui.h>
#include <armarx/navigation/core/time/ChronoMonotonicTimeServer.h>
@@ -59,8 +61,8 @@
#include <armarx/navigation/memory/client/stack_result/Writer.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
-#include <armarx/navigation/server/execution/PlatformUnitExecutor.h>
#include <armarx/navigation/server/introspection/ArvizIntrospector.h>
+#include <armarx/navigation/server/execution/PlatformControllerExecutor.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
#include <armarx/navigation/server/parameterization/MemoryParameterizationService.h>
@@ -83,7 +85,8 @@ namespace armarx::navigation::components
virtual public Component,
virtual public client::NavigatorInterface,
virtual public ObjectPoseClientPluginUser,
- virtual public ArVizComponentPluginUser
+ virtual public ArVizComponentPluginUser,
+ virtual public armarx::control::client::ComponentPluginUser
// virtual public armem::ListeningClientPluginUser
{
@@ -163,16 +166,12 @@ namespace armarx::navigation::components
private:
void visualizeSPFA();
- // TODO update context periodically
-
- // TODO: Remove dependency to PlatformUnit. This component is generally
- // capable to also control drones or the like...
- PlatformUnitInterfacePrx platformUnit;
RemoteGuiInterfacePrx remoteGui;
core::Scene scene;
- std::optional<server::PlatformUnitExecutor> executor;
+
+ std::optional<server::PlatformControllerExecutor> executor;
std::optional<server::ArvizIntrospector> introspector;
std::unordered_map<std::string, server::Navigator> navigators;
@@ -214,8 +213,6 @@ namespace armarx::navigation::components
{
float occupiedGridThreshold{0.55F};
- std::string robotName = "Armar6";
-
int robotUpdatePeriodMs = 10;
};
diff --git a/source/armarx/navigation/components/Navigator/RemoteGui.cpp b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
index 2f8afd27..37a191ba 100644
--- a/source/armarx/navigation/components/Navigator/RemoteGui.cpp
+++ b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
@@ -23,7 +23,6 @@
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/server/Navigator.h>
-#include <armarx/navigation/server/execution/PlatformUnitExecutor.h>
#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
#include <armarx/navigation/util/util.h>
--
GitLab
From 48428010b65dd8fabb713fe2ea60d39841404d99 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:43:50 +0200
Subject: [PATCH 13/47] cmake: + platform_controller
---
source/armarx/navigation/CMakeLists.txt | 1 +
1 file changed, 1 insertion(+)
diff --git a/source/armarx/navigation/CMakeLists.txt b/source/armarx/navigation/CMakeLists.txt
index 056cdb5a..9026c247 100644
--- a/source/armarx/navigation/CMakeLists.txt
+++ b/source/armarx/navigation/CMakeLists.txt
@@ -13,6 +13,7 @@ add_subdirectory(factories)
add_subdirectory(location)
add_subdirectory(memory)
add_subdirectory(server)
+add_subdirectory(platform_controller)
# Components.
add_subdirectory(components)
--
GitLab
From d7eb033b7ffc9a3a06ff6291f1683cc9b4319ff1 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:44:10 +0200
Subject: [PATCH 14/47] cmake: + dep armarx::control
---
CMakeLists.txt | 1 +
1 file changed, 1 insertion(+)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 072b8adb..49d3cada 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,6 +16,7 @@ armarx_project(navigation NAMESPACE armarx)
armarx_find_package(PUBLIC RobotAPI REQUIRED)
# - optional
+armarx_find_package(PUBLIC armarx::control)
armarx_find_package(PUBLIC ArmarXGui)
armarx_find_package(PUBLIC MemoryX QUIET)
armarx_find_package(PUBLIC VisionX QUIET)
--
GitLab
From 773b0618088787b211c6eb01fa7e2bae69bf5f86 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:44:52 +0200
Subject: [PATCH 15/47] scnearios: PlatformNavigation and NavigationSimulation
update
---
.../NavigationSimulation.scx | 2 +-
...ulatedObjectLocalizerDynamicSimulation.cfg | 8 --
.../config/ObjectMemory.cfg | 83 +++++++++++++++----
.../config/RobotStateMemory.cfg | 16 ----
.../config/RobotUnitSimulationApp.cfg | 2 +-
.../SelfLocalizationDynamicSimulationApp.cfg | 8 --
.../config/SimulatorApp.cfg | 2 +-
.../config/VisionMemory.cfg | 16 ----
.../PlatformNavigation/PlatformNavigation.scx | 2 +-
.../config/ObjectMemory.cfg | 83 +++++++++++++++----
.../config/VisionMemory.cfg | 16 ----
.../config/example_client.cfg | 10 +--
.../config/navigation_memory.cfg | 16 ----
.../PlatformNavigation/config/navigator.cfg | 36 ++------
14 files changed, 145 insertions(+), 155 deletions(-)
diff --git a/scenarios/NavigationSimulation/NavigationSimulation.scx b/scenarios/NavigationSimulation/NavigationSimulation.scx
index 152187c7..66b83347 100644
--- a/scenarios/NavigationSimulation/NavigationSimulation.scx
+++ b/scenarios/NavigationSimulation/NavigationSimulation.scx
@@ -45,7 +45,7 @@
<application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotStateMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="MotionPlanningServerApp" instance="" package="RobotComponents" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="LaserScannerPointCloudProviderApp" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <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"/>
</scenario>
diff --git a/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg b/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
index a64fd58d..7c4574e0 100644
--- a/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
+++ b/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
@@ -133,14 +133,6 @@ ArmarX.ArticulatedObjectLocalizerDynamicSimulation.mem.obj.articulated.ProviderN
# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.objects = Default value not mapped.
-# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.tpc.sub.MemoryListener = MemoryUpdates
-
-
# 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
diff --git a/scenarios/NavigationSimulation/config/ObjectMemory.cfg b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
index b949dad4..57b383ba 100644
--- a/scenarios/NavigationSimulation/config/ObjectMemory.cfg
+++ b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
@@ -142,14 +142,6 @@ ArmarX.ObjectMemory.MinimumLoggingLevel = Debug
# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
-# ArmarX.ObjectMemory.RemoteStateComponentName: Name of the robot state component
-# Attributes:
-# - Default: RobotStateComponent
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.RemoteStateComponentName = RobotStateComponent
-
-
# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
# Attributes:
# - Default: KinematicUnitObserver
@@ -487,6 +479,31 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.show: Show arrows linearly predicting object positions.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.show = 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
@@ -616,6 +633,38 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.mem.ltm.storagepath = Default value not mapped.
+# 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.descriptionSegment:
+# Attributes:
+# - Default: Description
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.descriptionSegment = Description
+
+
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment:
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment:
+# Attributes:
+# - Default: Proprioception
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment = Proprioception
+
+
# 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:
@@ -634,28 +683,28 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# ArmarX.ObjectMemory.prediction.TimeWindow: Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
# Attributes:
-# - Default: DebugObserver
+# - Default: 2
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
+# ArmarX.ObjectMemory.prediction.TimeWindow = 2
-# ArmarX.ObjectMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# ArmarX.ObjectMemory.robotName:
# Attributes:
-# - Default: MemoryUpdates
+# - Default: Armar6
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.tpc.pub.MemoryListener = MemoryUpdates
+# ArmarX.ObjectMemory.robotName = Armar6
-# ArmarX.ObjectMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
-# - Default: MemoryUpdates
+# - Default: DebugObserver
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.tpc.sub.MemoryListener = MemoryUpdates
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
diff --git a/scenarios/NavigationSimulation/config/RobotStateMemory.cfg b/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
index 5806a1ce..71c1ac65 100644
--- a/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
+++ b/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
@@ -435,22 +435,6 @@ ArmarX.RobotStateMemory.seg.localization.MaxHistorySize = 3000
ArmarX.RobotStateMemory.seg.proprioception.MaxHistorySize = 3000
-# ArmarX.RobotStateMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotStateMemory.tpc.pub.MemoryListener = MemoryUpdates
-
-
-# ArmarX.RobotStateMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotStateMemory.tpc.sub.MemoryListener = MemoryUpdates
-
-
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
# - Default: 0
diff --git a/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg b/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
index 5bd8e466..a02e6d4b 100644
--- a/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
+++ b/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
@@ -363,7 +363,7 @@ ArmarX.RobotUnitSimulation.InverseDynamicsRobotJointSets = PlatformRightArm
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.RobotUnitSimulation.MinimumLoggingLevel = Undefined
+ArmarX.RobotUnitSimulation.MinimumLoggingLevel = Debug
# ArmarX.RobotUnitSimulation.NjointController_AllowedExecutionTimePerControlDeviceUntilError: An Error will be printed, If the execution time in micro seconds of a NJointControllerBase exceeds this parameter times the number of ControlDevices.
diff --git a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
index 20d8ea06..23b096b3 100644
--- a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
+++ b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
@@ -299,14 +299,6 @@ ArmarX.SelfLocalizationDynamicSimulation.mem.robot_state.Memory = RobotState
# ArmarX.SelfLocalizationDynamicSimulation.tpc.pub.PlatformUnit = PlatformUnit
-# ArmarX.SelfLocalizationDynamicSimulation.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SelfLocalizationDynamicSimulation.tpc.sub.MemoryListener = MemoryUpdates
-
-
# ArmarX.SelfLocalizationDynamicSimulation.working_memory.update: If enabled, updates the global pose in the working memory
# Attributes:
# - Default: true
diff --git a/scenarios/NavigationSimulation/config/SimulatorApp.cfg b/scenarios/NavigationSimulation/config/SimulatorApp.cfg
index f5557e5a..8401b4d0 100644
--- a/scenarios/NavigationSimulation/config/SimulatorApp.cfg
+++ b/scenarios/NavigationSimulation/config/SimulatorApp.cfg
@@ -900,7 +900,7 @@ ArmarX.Simulator.SimulationType = kinematics
# - Default: 25
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Simulator.StepTimeMS = 25
+ArmarX.Simulator.StepTimeMS = 10
# ArmarX.Simulator.WorkingMemoryName: Name of WorkingMemory
diff --git a/scenarios/NavigationSimulation/config/VisionMemory.cfg b/scenarios/NavigationSimulation/config/VisionMemory.cfg
index 9c59a9f6..22e0d3e6 100644
--- a/scenarios/NavigationSimulation/config/VisionMemory.cfg
+++ b/scenarios/NavigationSimulation/config/VisionMemory.cfg
@@ -364,19 +364,3 @@
# ArmarX.VisionMemory.pointCloudMaxHistorySize = 20
-# ArmarX.VisionMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.tpc.pub.MemoryListener = MemoryUpdates
-
-
-# ArmarX.VisionMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.tpc.sub.MemoryListener = MemoryUpdates
-
-
diff --git a/scenarios/PlatformNavigation/PlatformNavigation.scx b/scenarios/PlatformNavigation/PlatformNavigation.scx
index 76fce009..3acc2c13 100644
--- a/scenarios/PlatformNavigation/PlatformNavigation.scx
+++ b/scenarios/PlatformNavigation/PlatformNavigation.scx
@@ -6,7 +6,7 @@
<application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="navigator" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="navigation_memory" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="example_client" instance="" package="armarx_navigation" nodeName="" enabled="false" 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"/>
</scenario>
diff --git a/scenarios/PlatformNavigation/config/ObjectMemory.cfg b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
index 2173b95e..5e0858b9 100644
--- a/scenarios/PlatformNavigation/config/ObjectMemory.cfg
+++ b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
@@ -142,14 +142,6 @@
# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
-# ArmarX.ObjectMemory.RemoteStateComponentName: Name of the robot state component
-# Attributes:
-# - Default: RobotStateComponent
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.RemoteStateComponentName = RobotStateComponent
-
-
# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
# Attributes:
# - Default: KinematicUnitObserver
@@ -487,6 +479,31 @@
# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.show: Show arrows linearly predicting object positions.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.show = 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
@@ -616,6 +633,38 @@
# ArmarX.ObjectMemory.mem.ltm.storagepath = Default value not mapped.
+# 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.descriptionSegment:
+# Attributes:
+# - Default: Description
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.descriptionSegment = Description
+
+
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment:
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment:
+# Attributes:
+# - Default: Proprioception
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment = Proprioception
+
+
# 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:
@@ -634,28 +683,28 @@
# ArmarX.ObjectMemory.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# ArmarX.ObjectMemory.prediction.TimeWindow: Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
# Attributes:
-# - Default: DebugObserver
+# - Default: 2
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
+# ArmarX.ObjectMemory.prediction.TimeWindow = 2
-# ArmarX.ObjectMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# ArmarX.ObjectMemory.robotName:
# Attributes:
-# - Default: MemoryUpdates
+# - Default: Armar6
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.tpc.pub.MemoryListener = MemoryUpdates
+# ArmarX.ObjectMemory.robotName = Armar6
-# ArmarX.ObjectMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
-# - Default: MemoryUpdates
+# - Default: DebugObserver
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.tpc.sub.MemoryListener = MemoryUpdates
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
diff --git a/scenarios/PlatformNavigation/config/VisionMemory.cfg b/scenarios/PlatformNavigation/config/VisionMemory.cfg
index 9c59a9f6..22e0d3e6 100644
--- a/scenarios/PlatformNavigation/config/VisionMemory.cfg
+++ b/scenarios/PlatformNavigation/config/VisionMemory.cfg
@@ -364,19 +364,3 @@
# ArmarX.VisionMemory.pointCloudMaxHistorySize = 20
-# ArmarX.VisionMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.tpc.pub.MemoryListener = MemoryUpdates
-
-
-# ArmarX.VisionMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.tpc.sub.MemoryListener = MemoryUpdates
-
-
diff --git a/scenarios/PlatformNavigation/config/example_client.cfg b/scenarios/PlatformNavigation/config/example_client.cfg
index e2d47a84..ba425c4c 100644
--- a/scenarios/PlatformNavigation/config/example_client.cfg
+++ b/scenarios/PlatformNavigation/config/example_client.cfg
@@ -154,7 +154,7 @@
# ArmarX.ExampleClient.nav.NavigatorName: Name of the Navigator
# Attributes:
-# - Default: Navigator
+# - Default: navigator
# - Case sensitivity: yes
# - Required: no
ArmarX.ExampleClient.nav.NavigatorName = navigator
@@ -176,14 +176,6 @@ ArmarX.ExampleClient.nav.NavigatorName = navigator
# 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;...
# Attributes:
# - Default: ""
diff --git a/scenarios/PlatformNavigation/config/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index 604df43d..8e727c26 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -323,22 +323,6 @@
ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/audimax-science-week-opening
-# ArmarX.NavigationMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.NavigationMemory.tpc.pub.MemoryListener = MemoryUpdates
-
-
-# ArmarX.NavigationMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.NavigationMemory.tpc.sub.MemoryListener = MemoryUpdates
-
-
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
# - Default: true
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index 75516bfe..cf79dbcd 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -142,12 +142,11 @@
ArmarX.Navigator.ObjectName = navigator
-# ArmarX.Navigator.cmp.PlatformUnit: Ice object name of the `PlatformUnit` component.
+# ArmarX.Navigator.RobotUnitName: Name of the RobotUnit
# Attributes:
-# - Default: PlatformUnit
# - Case sensitivity: yes
-# - Required: no
-ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
+# - Required: yes
+ArmarX.Navigator.RobotUnitName = Armar6Unit
# ArmarX.Navigator.cmp.RemoteGui: Ice object name of the `RemoteGui` component.
@@ -302,22 +301,6 @@ 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:
@@ -344,14 +327,6 @@ ArmarX.Navigator.mem.vision.occupancy_grid.Memory = Vision
ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
-# ArmarX.Navigator.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.Navigator.tpc.sub.MemoryListener = MemoryUpdates
-
-
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
# - Default: true
@@ -420,3 +395,8 @@ ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
ArmarX.Verbosity = Verbose
+# ArmarX.Navigator.cmp.PlatformUnit:
+# Attributes:
+ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
+
+
--
GitLab
From 032bf4cb9e97fa2e49fb009bc0229ebd72c40d2f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 21:27:21 +0200
Subject: [PATCH 16/47] discussion with SALt
---
.../components/example_client/Component.cpp | 2 -
...ulatedObjectLocalizerDynamicSimulation.cfg | 8 ++++
.../HandUnitDynamicSimulationApp.LeftHand.cfg | 17 -------
...HandUnitDynamicSimulationApp.RightHand.cfg | 17 -------
.../config/ObjectMemory.cfg | 48 ++++---------------
.../config/RobotStateMemory.cfg | 16 +++++++
.../SelfLocalizationDynamicSimulationApp.cfg | 18 ++++++-
.../config/VisionMemory.cfg | 16 +++++++
.../components/Navigator/Navigator.h | 3 +-
source/armarx/navigation/core/DynamicScene.h | 8 ++--
source/armarx/navigation/core/StaticScene.h | 4 +-
source/armarx/navigation/core/types.h | 3 --
.../global_planning/GlobalPlanner.h | 6 +--
.../local_planning/TimedElasticBands.h | 5 ++
source/armarx/navigation/server/Navigator.cpp | 3 +-
15 files changed, 84 insertions(+), 90 deletions(-)
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index 84f1485b..1479ad6d 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -125,8 +125,6 @@ namespace armarx::navigation::components::example_client
getNavigator().moveTo(goal, core::NavigationFrame::Absolute);
- Clock::WaitFor(Duration::Seconds(15));
-
// Wait until goal is reached
armarx::navigation::client::StopEvent se = getNavigator().waitForStop();
if (se)
diff --git a/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg b/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
index 7c4574e0..a64fd58d 100644
--- a/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
+++ b/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
@@ -133,6 +133,14 @@ ArmarX.ArticulatedObjectLocalizerDynamicSimulation.mem.obj.articulated.ProviderN
# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.objects = Default value not mapped.
+# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.tpc.sub.MemoryListener = MemoryUpdates
+
+
# 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
diff --git a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
index aa21b8fd..18e2d2db 100644
--- a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
+++ b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
@@ -139,23 +139,6 @@ ArmarX.HandUnitDynamicSimulation.ObjectName = LeftHandUnit
# ArmarX.HandUnitDynamicSimulation.SimulatorProxyName = Simulator
-# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory: Require the legacy MemoryX working memory to be available before starting.
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory = false
-
-
-# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName: Name of the object pose storage (only used if necessary).
-# Attributes:
-# - Default: ObjectMemory
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName = ObjectMemory
-
-
# ArmarX.HandUnitDynamicSimulation.inheritFrom: No Description
# Attributes:
# - Default: RobotConfig
diff --git a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
index b92bb000..85cc4345 100644
--- a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
+++ b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
@@ -139,23 +139,6 @@ ArmarX.HandUnitDynamicSimulation.ObjectName = RightHandUnit
# ArmarX.HandUnitDynamicSimulation.SimulatorProxyName = Simulator
-# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory: Require the legacy MemoryX working memory to be available before starting.
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory = false
-
-
-# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName: Name of the object pose storage (only used if necessary).
-# Attributes:
-# - Default: ObjectMemory
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName = ObjectMemory
-
-
# ArmarX.HandUnitDynamicSimulation.inheritFrom: No Description
# Attributes:
# - Default: RobotConfig
diff --git a/scenarios/NavigationSimulation/config/ObjectMemory.cfg b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
index 57b383ba..58cd471b 100644
--- a/scenarios/NavigationSimulation/config/ObjectMemory.cfg
+++ b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
@@ -142,6 +142,14 @@ ArmarX.ObjectMemory.MinimumLoggingLevel = Debug
# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
+# ArmarX.ObjectMemory.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.RemoteStateComponentName = RobotStateComponent
+
+
# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
# Attributes:
# - Default: KinematicUnitObserver
@@ -633,38 +641,6 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.mem.ltm.storagepath = Default value not mapped.
-# 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.descriptionSegment:
-# Attributes:
-# - Default: Description
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.robot_state.descriptionSegment = Description
-
-
-# ArmarX.ObjectMemory.mem.robot_state.localizationSegment:
-# Attributes:
-# - Default: Localization
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.robot_state.localizationSegment = Localization
-
-
-# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment:
-# Attributes:
-# - Default: Proprioception
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment = Proprioception
-
-
# 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:
@@ -691,14 +667,6 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.prediction.TimeWindow = 2
-# ArmarX.ObjectMemory.robotName:
-# Attributes:
-# - Default: Armar6
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.robotName = Armar6
-
-
# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
# - Default: DebugObserver
diff --git a/scenarios/NavigationSimulation/config/RobotStateMemory.cfg b/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
index 71c1ac65..5806a1ce 100644
--- a/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
+++ b/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
@@ -435,6 +435,22 @@ ArmarX.RobotStateMemory.seg.localization.MaxHistorySize = 3000
ArmarX.RobotStateMemory.seg.proprioception.MaxHistorySize = 3000
+# ArmarX.RobotStateMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.tpc.pub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.RobotStateMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.tpc.sub.MemoryListener = MemoryUpdates
+
+
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
# - Default: 0
diff --git a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
index 23b096b3..1dab8f00 100644
--- a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
+++ b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
@@ -175,6 +175,14 @@ ArmarX.SelfLocalizationDynamicSimulation.RobotName = Armar6
# ArmarX.SelfLocalizationDynamicSimulation.cmp.Simulator = Simulator
+# ArmarX.SelfLocalizationDynamicSimulation.cmp.WorkingMemory: Ice object name of the `WorkingMemory` component.
+# Attributes:
+# - Default: WorkingMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SelfLocalizationDynamicSimulation.cmp.WorkingMemory = WorkingMemory
+
+
# ArmarX.SelfLocalizationDynamicSimulation.cycleTime:
# Attributes:
# - Default: 30
@@ -183,7 +191,7 @@ ArmarX.SelfLocalizationDynamicSimulation.RobotName = Armar6
# ArmarX.SelfLocalizationDynamicSimulation.cycleTime = 30
-# ArmarX.SelfLocalizationDynamicSimulation.longterm_memory: Which legacy long-term memory to use if longterm_memory.updateor longterm_memory.retrieve_initial_pose are set
+# ArmarX.SelfLocalizationDynamicSimulation.longterm_memory: Ice object name of the `LongtermMemory` component.
# Attributes:
# - Default: LongtermMemory
# - Case sensitivity: yes
@@ -299,6 +307,14 @@ ArmarX.SelfLocalizationDynamicSimulation.mem.robot_state.Memory = RobotState
# ArmarX.SelfLocalizationDynamicSimulation.tpc.pub.PlatformUnit = PlatformUnit
+# ArmarX.SelfLocalizationDynamicSimulation.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SelfLocalizationDynamicSimulation.tpc.sub.MemoryListener = MemoryUpdates
+
+
# ArmarX.SelfLocalizationDynamicSimulation.working_memory.update: If enabled, updates the global pose in the working memory
# Attributes:
# - Default: true
diff --git a/scenarios/NavigationSimulation/config/VisionMemory.cfg b/scenarios/NavigationSimulation/config/VisionMemory.cfg
index 22e0d3e6..9c59a9f6 100644
--- a/scenarios/NavigationSimulation/config/VisionMemory.cfg
+++ b/scenarios/NavigationSimulation/config/VisionMemory.cfg
@@ -364,3 +364,19 @@
# ArmarX.VisionMemory.pointCloudMaxHistorySize = 20
+# ArmarX.VisionMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.tpc.pub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.VisionMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.tpc.sub.MemoryListener = MemoryUpdates
+
+
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 618b378d..3883361f 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -147,14 +147,13 @@ namespace armarx::navigation::components
/// @see armarx::ManagedIceObject::onConnectComponent()
void onConnectComponent() override;
- void onReconnectComponent();
-
/// @see armarx::ManagedIceObject::onDisconnectComponent()
void onDisconnectComponent() override;
/// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
+ void onReconnectComponent();
void initializeScene();
core::StaticScene staticScene();
diff --git a/source/armarx/navigation/core/DynamicScene.h b/source/armarx/navigation/core/DynamicScene.h
index 1639a726..05f211a9 100644
--- a/source/armarx/navigation/core/DynamicScene.h
+++ b/source/armarx/navigation/core/DynamicScene.h
@@ -22,16 +22,18 @@
#pragma once
-#include <memory>
+
namespace armarx::navigation::core
{
- class DynamicScene
+ struct DynamicScene
{
+
+ // TODO(SALt): Implement
+
public:
protected:
private:
};
- using DynamicScenePtr = std::shared_ptr<DynamicScene>;
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/StaticScene.h b/source/armarx/navigation/core/StaticScene.h
index 1f4ad33a..de0f008d 100644
--- a/source/armarx/navigation/core/StaticScene.h
+++ b/source/armarx/navigation/core/StaticScene.h
@@ -34,9 +34,11 @@ namespace armarx::navigation::core
struct StaticScene
{
VirtualRobot::SceneObjectSetPtr objects;
+
+ // TODO(fabian.reister): rename, why unique_ptr
std::unique_ptr<algorithms::Costmap> costmap;
+
};
- using StaticScenePtr = std::shared_ptr<StaticScene>;
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/types.h b/source/armarx/navigation/core/types.h
index 3b5df564..3cc08512 100644
--- a/source/armarx/navigation/core/types.h
+++ b/source/armarx/navigation/core/types.h
@@ -84,10 +84,7 @@ namespace armarx::navigation::core
// TopologicMapPtr topologicMap;
VirtualRobot::RobotPtr robot;
- std::optional<core::Twist> platformVelocity;
-
std::optional<core::SceneGraph> graph;
-
TimeServerInterface* timeServer;
};
diff --git a/source/armarx/navigation/global_planning/GlobalPlanner.h b/source/armarx/navigation/global_planning/GlobalPlanner.h
index 82983619..1a028db1 100644
--- a/source/armarx/navigation/global_planning/GlobalPlanner.h
+++ b/source/armarx/navigation/global_planning/GlobalPlanner.h
@@ -42,9 +42,9 @@ namespace armarx::navigation::global_planning
/**
- * @brief Parameters for GlobalPlanner
- *
- */
+ * @brief Parameters for GlobalPlanner
+ *
+ */
struct GlobalPlannerParams
{
bool foo;
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index e43747cd..c8b84a21 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -24,16 +24,19 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include "armarx/navigation/core/Trajectory.h"
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/local_planning/core.h>
namespace armarx::navigation::loc_plan
{
+ // 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);
@@ -47,6 +50,8 @@ namespace armarx::navigation::loc_plan
TimedElasticBands(const Params& params, const core::Scene& ctx);
~TimedElasticBands() override = default;
+ void init(const core::Trajectory& initialTrajectory);
+
std::optional<LocalPlannerResult> plan(const core::Trajectory& goal) override;
protected:
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index bf343cba..5581f4d5 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -626,7 +626,7 @@ namespace armarx::navigation::server
ARMARX_INFO << "Planning local trajectory enabled";
replanningTask =
new PeriodicTask<Navigator>(this,
- &Navigator::updateLocalPlanner,
+ &Navigator::replan,
config.general.tasks.replanningUpdatePeriod,
false,
"LocalPlannerTask");
@@ -759,6 +759,7 @@ namespace armarx::navigation::server
void
Navigator::replan()
{
+ updateScene();
updateLocalPlanner();
updateExecutor();
updateIntrospector();
--
GitLab
From 4ca2dc10518b7a57fc30579f921b4dca3403027e Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 21:28:38 +0200
Subject: [PATCH 17/47] scneario update
---
scenarios/PlatformNavigation/PlatformNavigation.scx | 3 ++-
scenarios/PlatformNavigation/config/navigator.cfg | 13 ++++++++-----
2 files changed, 10 insertions(+), 6 deletions(-)
diff --git a/scenarios/PlatformNavigation/PlatformNavigation.scx b/scenarios/PlatformNavigation/PlatformNavigation.scx
index 3acc2c13..1f20100c 100644
--- a/scenarios/PlatformNavigation/PlatformNavigation.scx
+++ b/scenarios/PlatformNavigation/PlatformNavigation.scx
@@ -6,7 +6,8 @@
<application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="navigator" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="navigation_memory" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="example_client" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="example_client" instance="" package="armarx_navigation" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="VisionMemory" instance="" package="VisionX" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="control_memory" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index cf79dbcd..34ddef29 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -149,6 +149,14 @@ ArmarX.Navigator.ObjectName = navigator
ArmarX.Navigator.RobotUnitName = Armar6Unit
+# ArmarX.Navigator.cmp.PlatformUnit: No Description
+# Attributes:
+# - Default: Armar6PlatformUnit
+# - Case sensitivity: no
+# - Required: no
+ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
+
+
# ArmarX.Navigator.cmp.RemoteGui: Ice object name of the `RemoteGui` component.
# Attributes:
# - Default: RemoteGuiProvider
@@ -395,8 +403,3 @@ ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
ArmarX.Verbosity = Verbose
-# ArmarX.Navigator.cmp.PlatformUnit:
-# Attributes:
-ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
-
-
--
GitLab
From 35b6ff47a7e29e9ce801501e18b80cd4a42c61c5 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 21:29:00 +0200
Subject: [PATCH 18/47] scenario update
---
.../config/control_memory.cfg | 375 ++++++++++++++++++
1 file changed, 375 insertions(+)
create mode 100644 scenarios/PlatformNavigation/config/control_memory.cfg
diff --git a/scenarios/PlatformNavigation/config/control_memory.cfg b/scenarios/PlatformNavigation/config/control_memory.cfg
new file mode 100644
index 00000000..0d5c0948
--- /dev/null
+++ b/scenarios/PlatformNavigation/config/control_memory.cfg
@@ -0,0 +1,375 @@
+# ==================================================================
+# 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..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm..buffer.storeFreq = 10
+
+
+# ArmarX.ControlMemory.mem.ltm.depthImageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.depthImageExtractor.Enabled = true
+
+
+# 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.mem.ltm.exrConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.exrConverter.Enabled = true
+
+
+# ArmarX.ControlMemory.mem.ltm.imageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.imageExtractor.Enabled = true
+
+
+# ArmarX.ControlMemory.mem.ltm.memFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.memFreqFilter.Enabled = false
+
+
+# ArmarX.ControlMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm.memFreqFilter.WaitingTime = -1
+
+
+# ArmarX.ControlMemory.mem.ltm.pngConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.pngConverter.Enabled = true
+
+
+# ArmarX.ControlMemory.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.ControlMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
+
+
+# ArmarX.ControlMemory.mem.ltm.snapEqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.snapEqFilter.Enabled = false
+
+
+# ArmarX.ControlMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
+
+
+# ArmarX.ControlMemory.mem.ltm.snapFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.snapFreqFilter.Enabled = false
+
+
+# ArmarX.ControlMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
+
+
+# ArmarX.ControlMemory.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.ControlMemory.mem.ltm.storagepath = Default value not mapped.
+
+
+# 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
+
+
--
GitLab
From 93cfea39bfc1288217289fc466e816e818186cff Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 21:29:13 +0200
Subject: [PATCH 19/47] minor
---
examples/components/example_client/Component.cpp | 5 ++---
1 file changed, 2 insertions(+), 3 deletions(-)
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index 1479ad6d..aa58e606 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -290,10 +290,9 @@ namespace armarx::navigation::components::example_client
ARMARX_INFO << "Moving to goal pose";
// Start moving to goal position using above config.
- auto& virtualRobotReader = virtualRobotReaderPlugin->get();
+ auto& virtualRobotReader = virtualRobotReaderPlugin->get();
- auto robot =
- virtualRobotReader.getSynchronizedRobot(properties.robotName, armarx::Clock::Now());
+ auto robot = virtualRobotReader.getSynchronizedRobot(properties.robotName);
ARMARX_CHECK_NOT_NULL(robot);
const core::Pose initialPose(robot->getGlobalPose());
--
GitLab
From d17f3f6c325d9061f4229780951fce6c23acf3c0 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 21:29:29 +0200
Subject: [PATCH 20/47] platform controller update
---
.../controller_descriptions.h | 4 ++-
.../platform_controller/json_conversions.cpp | 6 ++++
.../platform_controller/json_conversions.h | 30 +++++++++++++++++++
3 files changed, 39 insertions(+), 1 deletion(-)
create mode 100644 source/armarx/navigation/platform_controller/json_conversions.cpp
create mode 100644 source/armarx/navigation/platform_controller/json_conversions.h
diff --git a/source/armarx/navigation/platform_controller/controller_descriptions.h b/source/armarx/navigation/platform_controller/controller_descriptions.h
index 4fa54a75..d352bed1 100644
--- a/source/armarx/navigation/platform_controller/controller_descriptions.h
+++ b/source/armarx/navigation/platform_controller/controller_descriptions.h
@@ -22,8 +22,8 @@
#pragma once
#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>
namespace armarx::control::client
@@ -34,6 +34,8 @@ namespace armarx::control::client
using AronDTO =
armarx::navigation::platform_controller::platform_trajectory::arondto::Config;
+ using BO = armarx::navigation::platform_controller::platform_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;
diff --git a/source/armarx/navigation/platform_controller/json_conversions.cpp b/source/armarx/navigation/platform_controller/json_conversions.cpp
new file mode 100644
index 00000000..71347775
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/json_conversions.cpp
@@ -0,0 +1,6 @@
+#include "json_conversions.h"
+
+namespace armarx_navigation::platform_controller
+{
+
+}
diff --git a/source/armarx/navigation/platform_controller/json_conversions.h b/source/armarx/navigation/platform_controller/json_conversions.h
new file mode 100644
index 00000000..6abd40df
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/json_conversions.h
@@ -0,0 +1,30 @@
+/**
+ * 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 <SimoxUtility/json.h>
+
+
+namespace armarx::navigation::platform_controller::platform_trajectory
+{
+
+} // namespace armarx::navigation::platform_controller::platform_trajectory
--
GitLab
From 7f5b77b0df9acd5e127e505d1fc78c6ba675cc5d Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 21:29:42 +0200
Subject: [PATCH 21/47] controller configs
---
.../PlatformTrajectory/default.json | 23 +++++++++++++++++++
1 file changed, 23 insertions(+)
create mode 100644 data/armarx_navigation/controller_config/PlatformTrajectory/default.json
diff --git a/data/armarx_navigation/controller_config/PlatformTrajectory/default.json b/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
new file mode 100644
index 00000000..4cd38813
--- /dev/null
+++ b/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
@@ -0,0 +1,23 @@
+{
+ "params": {
+ "pidPos": {
+ "Kp": 1,
+ "Ki": 0,
+ "Kd": 0
+ },
+ "pidOri": {
+ "Kp": 1,
+ "Ki": 0,
+ "Kd": 0
+ },
+ "limits": {
+ "linear": 300,
+ "angular": 0.2
+ }
+ },
+ "targets": {
+ "trajectory": {
+ "points": []
+ }
+ }
+}
--
GitLab
From 56fdef2eeb23930f8493b4e80fa5d321241a1e70 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 21:29:51 +0200
Subject: [PATCH 22/47] server/execution
---
.../server/execution/ExecutorInterface.h | 2 +
.../execution/PlatformControllerExecutor.cpp | 50 +++++++++++++++++--
.../execution/PlatformControllerExecutor.h | 6 +--
3 files changed, 51 insertions(+), 7 deletions(-)
diff --git a/source/armarx/navigation/server/execution/ExecutorInterface.h b/source/armarx/navigation/server/execution/ExecutorInterface.h
index c4a38f29..ea77fd7a 100644
--- a/source/armarx/navigation/server/execution/ExecutorInterface.h
+++ b/source/armarx/navigation/server/execution/ExecutorInterface.h
@@ -18,6 +18,8 @@ namespace armarx::navigation::server
virtual ~ExecutorInterface() = default;
virtual void execute(const core::Trajectory& trajectory) = 0;
+
+ virtual void start() = 0;
virtual void stop() = 0;
};
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
index 90acc53b..ed6d813e 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -1,10 +1,17 @@
#include "PlatformControllerExecutor.h"
+#include "ArmarXCore/core/PackagePath.h"
+#include "ArmarXCore/core/system/ArmarXDataPath.h"
+
#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/controller_descriptions.h>
+#include <armarx/navigation/platform_controller/aron_conversions.h>
+#include <armarx/navigation/platform_controller/json_conversions.h>
namespace armarx::navigation::server
@@ -17,13 +24,31 @@ namespace armarx::navigation::server
controllerComponentPlugin.getRobotUnitPlugin().getRobotUnit()->loadLibFromPackage(
"armarx_navigation", "libarmarx_navigation_platform_controller.so");
+ // make default configs available to the memory
+ ARMARX_INFO << "Loading default configs";
+ {
+ const std::string configBasePath =
+ PackagePath("armarx_navigation", "controller_config").toSystemPath();
+
+ armarx::control::memory::config::parseAndStoreDefaultConfigs<
+ armarx::navigation::common::ControllerType::PlatformTrajectory>(
+ configBasePath, controllerComponentPlugin.configMemoryWriter());
+
+ ARMARX_INFO << "asdlfasfdlh";
+ }
- auto builder = controllerPlugin.createControllerBuilder<
- armarx::navigation::common::ControllerType::PlatformTrajectory>();
+ // initialize controller
+ ARMARX_INFO << "Initializing controller";
+ {
+ auto builder = controllerPlugin.createControllerBuilder<
+ armarx::navigation::common::ControllerType::PlatformTrajectory>();
- ctrl.emplace(builder.create().value());
+ auto ctrlWrapper = builder.create();
+ ctrl.emplace(std::move(ctrlWrapper.value()));
+ }
ARMARX_CHECK(ctrl.has_value());
+ ARMARX_INFO << "PlatformControllerExecutor: init done.";
}
PlatformControllerExecutor::~PlatformControllerExecutor() = default;
@@ -32,5 +57,22 @@ namespace armarx::navigation::server
void
PlatformControllerExecutor::execute(const core::Trajectory& trajectory)
{
+ toAron(ctrl->config.targets.trajectory, trajectory);
+
+ // sends the updated config to the controller and stores it in the memory
+ ctrl->updateConfig();
+ }
+
+ void
+ PlatformControllerExecutor::start()
+ {
+ // TODO check whether the controller must be resetted (trajectory)
+ ctrl->activate();
+ }
+
+ void
+ PlatformControllerExecutor::stop()
+ {
+ ctrl->deactivate();
}
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
index 1b2ec239..d256904c 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
@@ -33,9 +33,9 @@ namespace armarx::navigation::server
~PlatformControllerExecutor() override;
void execute(const core::Trajectory& trajectory) override;
- void stop() override{
- // FIXME implement
- };
+
+ void start() override;
+ void stop() override;
private:
std::optional<armarx::control::client::ControllerWrapper<
--
GitLab
From e025bcb722d0b52d0a630a5beede390c310b5bb1 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 21:30:19 +0200
Subject: [PATCH 23/47] fix: sync robot
---
source/armarx/navigation/components/Navigator/Navigator.cpp | 1 -
1 file changed, 1 deletion(-)
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 5c4f0337..008ab81c 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -223,7 +223,6 @@ namespace armarx::navigation::components
ARMARX_TRACE;
auto robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(
getControlComponentPlugin().getRobotUnitPlugin().getRobotUnit()->getKinematicUnit()->getRobotName(),
- armarx::core::time::Clock::Now(),
VirtualRobot::RobotIO::RobotDescription::eCollisionModel,
true);
--
GitLab
From aa932d1c0dc66757932d3f45759aa15191c19325 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 21:30:28 +0200
Subject: [PATCH 24/47] json conversion
---
.../navigation/core/json_conversions.cpp | 25 -------------
.../armarx/navigation/core/json_conversions.h | 37 ++++++++++++++-----
2 files changed, 28 insertions(+), 34 deletions(-)
diff --git a/source/armarx/navigation/core/json_conversions.cpp b/source/armarx/navigation/core/json_conversions.cpp
index 21b6cd17..307b8bde 100644
--- a/source/armarx/navigation/core/json_conversions.cpp
+++ b/source/armarx/navigation/core/json_conversions.cpp
@@ -20,28 +20,3 @@
*/
#include "json_conversions.h"
-
-#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-
-namespace armarx::navigation::core
-{
-
- void
- arondto::to_json(nlohmann::json& j, const Edge& bo)
- {
- armarx::aron::data::writer::NlohmannJSONWriter writer;
- j = bo.write(writer);
- }
-
-
- void
- arondto::from_json(const nlohmann::json& j, Edge& bo)
- {
- armarx::aron::data::reader::NlohmannJSONReader reader;
- bo.read(reader, j);
- }
-
-
-} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/json_conversions.h b/source/armarx/navigation/core/json_conversions.h
index a8fd2caa..1d9d7298 100644
--- a/source/armarx/navigation/core/json_conversions.h
+++ b/source/armarx/navigation/core/json_conversions.h
@@ -23,14 +23,33 @@
#include <SimoxUtility/json/json.hpp>
-#include <armarx/navigation/core/aron/Graph.aron.generated.h>
+#include "RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronGeneratedClass.h"
+#include "RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h"
+#include "RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h"
-
-namespace armarx::navigation::core::arondto
+namespace armarx
{
-
- void to_json(nlohmann::json& j, const arondto::Edge& bo);
- void from_json(const nlohmann::json& j, arondto::Edge& bo);
-
-
-} // namespace armarx::navigation::core::arondto
+ template <typename AronDTO>
+ void
+ from_json(const nlohmann::json& j, AronDTO& dto)
+ {
+ static_assert(
+ std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass, AronDTO>::value);
+
+ armarx::aron::data::reader::NlohmannJSONReader reader;
+ dto.read(reader, j);
+ }
+
+
+ template <typename AronDTO>
+ void
+ to_json(nlohmann::json& j, const AronDTO& bo)
+ {
+ static_assert(
+ std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass, AronDTO>::value);
+
+ armarx::aron::data::writer::NlohmannJSONWriter writer;
+ j = bo.write(writer);
+ }
+
+} // namespace armarx
--
GitLab
From 8d05593c6e78b34682104cc5b96ef3c74489fb5d Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 22:52:19 +0200
Subject: [PATCH 25/47] scene provider /updater
---
.../server/scene_provider/SceneProvider.cpp | 104 ++++++++++++++++++
.../server/scene_provider/SceneProvider.h | 84 ++++++++++++++
.../scene_provider/SceneProviderInterface.h | 46 ++++++++
3 files changed, 234 insertions(+)
create mode 100644 source/armarx/navigation/server/scene_provider/SceneProvider.cpp
create mode 100644 source/armarx/navigation/server/scene_provider/SceneProvider.h
create mode 100644 source/armarx/navigation/server/scene_provider/SceneProviderInterface.h
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
new file mode 100644
index 00000000..04412bb3
--- /dev/null
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
@@ -0,0 +1,104 @@
+#include "SceneProvider.h"
+
+#include <VirtualRobot/SceneObjectSet.h>
+
+#include <armarx/navigation/algorithms/CostmapBuilder.h>
+#include <armarx/navigation/util/util.h>
+
+namespace armarx::navigation::server::scene_provider
+{
+
+ SceneProvider::SceneProvider(const InjectedServices& srv, const Config& config) :
+ srv(srv), config(config)
+ {
+ }
+
+ const core::Scene&
+ SceneProvider::scene() const
+ {
+ return scn;
+ }
+
+ bool
+ SceneProvider::initialize(const DateTime& timestamp)
+ {
+ scn.timestamp = timestamp;
+
+ scn.robot = srv.virtualRobotReader->getRobot(
+ config.robotName, timestamp, VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+ scn.staticScene = getStaticScene(timestamp);
+ scn.dynamicScene = getDynamicScene(timestamp);
+ scn.graph = getSceneGraph(timestamp);
+
+ return true; // TODO(fabian.reister): return false if sync fails
+ }
+
+ bool
+ SceneProvider::synchronize(const DateTime& timestamp)
+ {
+ scn.timestamp = timestamp;
+
+ ARMARX_CHECK_NOT_NULL(srv.virtualRobotReader);
+ srv.virtualRobotReader->synchronizeRobot(*scn.robot, timestamp);
+
+ scn.dynamicScene = getDynamicScene(timestamp);
+ scn.graph = getSceneGraph(timestamp);
+
+ return true; // TODO(fabian.reister): return false if sync fails
+ }
+
+ core::StaticScene
+ SceneProvider::getStaticScene(const DateTime& timestamp) const
+ {
+ ARMARX_TRACE;
+
+ const objpose::ObjectPoseSeq objectPoses = srv.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(scn.robot);
+
+ // 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");
+
+ const auto costmap = costmapBuilder.create();
+
+ // ARMARX_INFO << "Storing costmap in memory";
+ // costmapWriterPlugin->get().store(
+ // costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
+
+ ARMARX_INFO << "Done";
+
+ ARMARX_TRACE;
+
+ return {.objects = objects, .costmap = std::make_unique<algorithms::Costmap>(costmap)};
+ }
+
+ core::DynamicScene
+ SceneProvider::getDynamicScene(const DateTime& timestamp) const
+ {
+ return {}; // FIXME implement
+ }
+
+ core::SceneGraph
+ SceneProvider::getSceneGraph(const DateTime& /*timestamp*/) const
+ {
+ ARMARX_CHECK_NOT_NULL(srv.graphReader);
+ return {.subgraphs = srv.graphReader->graphs()};
+ }
+
+} // namespace armarx::navigation::server::scene_provider
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.h b/source/armarx/navigation/server/scene_provider/SceneProvider.h
new file mode 100644
index 00000000..cd93e072
--- /dev/null
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.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 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/VirtualRobot.h>
+
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+#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/server/scene_provider/SceneProviderInterface.h>
+
+
+namespace armarx::navigation::server::scene_provider
+{
+
+ class SceneProvider : virtual public SceneProviderInterface
+ {
+ public:
+ struct InjectedServices
+ {
+
+ memory::client::graph::Reader* graphReader;
+
+ memory::client::costmap::Reader* costmapReader;
+ // armem::vision::occupancy_grid::client::Reader occupancyGridReader;
+
+ // `robot_state` memory reader and writer
+ armem::robot_state::VirtualRobotReader* virtualRobotReader;
+
+ objpose::ObjectPoseClient objectPoseClient;
+ };
+
+ struct Config
+ {
+ std::string robotName;
+ };
+
+ SceneProvider(const InjectedServices& srv, const Config& config);
+
+ const core::Scene& scene() const override;
+
+ bool initialize(const DateTime& timestamp) override;
+ bool synchronize(const DateTime& timestamp) override;
+
+
+ private:
+ VirtualRobot::RobotPtr getRobot(const DateTime& timestamp) const;
+ core::StaticScene getStaticScene(const DateTime& timestamp) const;
+ core::DynamicScene getDynamicScene(const DateTime& timestamp) const;
+ core::SceneGraph getSceneGraph(const DateTime& timestamp) const;
+
+ InjectedServices srv;
+ Config config;
+
+ core::Scene scn;
+ };
+} // namespace armarx::navigation::server::scene_provider
diff --git a/source/armarx/navigation/server/scene_provider/SceneProviderInterface.h b/source/armarx/navigation/server/scene_provider/SceneProviderInterface.h
new file mode 100644
index 00000000..10869aab
--- /dev/null
+++ b/source/armarx/navigation/server/scene_provider/SceneProviderInterface.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 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/time/DateTime.h>
+
+namespace armarx::navigation::core
+{
+ struct Scene;
+}
+
+namespace armarx::navigation::server::scene_provider
+{
+
+ class SceneProviderInterface
+ {
+ public:
+ virtual const core::Scene& scene() const = 0;
+
+ virtual bool initialize(const DateTime& timestamp) = 0;
+ virtual bool synchronize(const DateTime& timestamp) = 0;
+
+
+ // non-api
+ virtual ~SceneProviderInterface() = default;
+ };
+} // namespace armarx::navigation::server::scene_provider
--
GitLab
From 3b2cc42dd53921b16c988c7bf57a13dee308fef1 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 22:53:43 +0200
Subject: [PATCH 26/47] prettifying includes
---
.../components/example_client/Component.cpp | 12 +++++------
.../components/example_client/Component.h | 4 ++--
.../algorithms/aron_conversions.cpp | 10 +++++-----
.../navigation/algorithms/aron_conversions.h | 2 +-
.../test/algorithms_costmap_test.cpp | 4 ++--
source/armarx/navigation/algorithms/util.cpp | 2 +-
.../client/services/MemorySubscriber.cpp | 4 ++--
.../NavigationMemory/NavigationMemory.cpp | 4 ++--
.../components/Navigator/Navigator.cpp | 9 +++++----
.../components/Navigator/Navigator.h | 12 +++++------
.../components/Navigator/RemoteGui.cpp | 2 +-
.../Component.cpp | 20 +++++++++----------
.../Component.h | 12 +++++------
.../navigation/core/aron_conversions.cpp | 2 +-
.../armarx/navigation/core/json_conversions.h | 6 +++---
source/armarx/navigation/graph/constants.h | 2 +-
.../local_planning/TimedElasticBands.h | 2 +-
source/armarx/navigation/location/constants.h | 2 +-
.../memory/client/costmap/Reader.cpp | 6 +++---
.../navigation/memory/client/costmap/Reader.h | 2 +-
.../memory/client/costmap/Writer.cpp | 6 +++---
.../navigation/memory/client/costmap/Writer.h | 2 +-
.../memory/client/events/Writer.cpp | 2 +-
.../memory/client/parameterization/Reader.cpp | 2 +-
.../PlatformTrajectoryController.cpp | 8 ++++----
.../PlatformTrajectoryController.h | 2 +-
source/armarx/navigation/server/Navigator.cpp | 4 +++-
.../execution/PlatformControllerExecutor.cpp | 4 ++--
.../introspection/ArvizIntrospector.cpp | 2 +-
.../TrajectoryFollowingController.cpp | 2 +-
30 files changed, 78 insertions(+), 75 deletions(-)
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index aa58e606..29001741 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -30,14 +30,14 @@
#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/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/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>
diff --git a/examples/components/example_client/Component.h b/examples/components/example_client/Component.h
index 0fb9bd4c..295d1964 100644
--- a/examples/components/example_client/Component.h
+++ b/examples/components/example_client/Component.h
@@ -27,8 +27,8 @@
#include <ArmarXCore/util/tasks.h>
#include <armarx/navigation/client.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/ReaderWriterPlugin.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
diff --git a/source/armarx/navigation/algorithms/aron_conversions.cpp b/source/armarx/navigation/algorithms/aron_conversions.cpp
index 155ae332..031170ef 100644
--- a/source/armarx/navigation/algorithms/aron_conversions.cpp
+++ b/source/armarx/navigation/algorithms/aron_conversions.cpp
@@ -1,12 +1,12 @@
#include "aron_conversions.h"
-#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/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 "armarx/navigation/algorithms/Costmap.h"
-#include "armarx/navigation/conversions/eigen.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/conversions/eigen.h>
namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/aron_conversions.h b/source/armarx/navigation/algorithms/aron_conversions.h
index b595e5bc..4c91b833 100644
--- a/source/armarx/navigation/algorithms/aron_conversions.h
+++ b/source/armarx/navigation/algorithms/aron_conversions.h
@@ -21,7 +21,7 @@
#pragma once
-#include "RobotAPI/libraries/armem/core/wm/memory_definitions.h"
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
diff --git a/source/armarx/navigation/algorithms/test/algorithms_costmap_test.cpp b/source/armarx/navigation/algorithms/test/algorithms_costmap_test.cpp
index 8b9dd842..5a721c71 100644
--- a/source/armarx/navigation/algorithms/test/algorithms_costmap_test.cpp
+++ b/source/armarx/navigation/algorithms/test/algorithms_costmap_test.cpp
@@ -22,8 +22,8 @@
*/
-#include "armarx/navigation/algorithms/persistence.h"
-#include "armarx/navigation/algorithms/util.h"
+#include <armarx/navigation/algorithms/persistence.h>
+#include <armarx/navigation/algorithms/util.h>
#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::algorithms
#define ARMARX_BOOST_TEST
diff --git a/source/armarx/navigation/algorithms/util.cpp b/source/armarx/navigation/algorithms/util.cpp
index 2918d94b..fe71db5a 100644
--- a/source/armarx/navigation/algorithms/util.cpp
+++ b/source/armarx/navigation/algorithms/util.cpp
@@ -39,7 +39,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include "armarx/navigation/algorithms/persistence.h"
+#include <armarx/navigation/algorithms/persistence.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/algorithms/types.h>
diff --git a/source/armarx/navigation/client/services/MemorySubscriber.cpp b/source/armarx/navigation/client/services/MemorySubscriber.cpp
index 89769e9f..ed515f00 100644
--- a/source/armarx/navigation/client/services/MemorySubscriber.cpp
+++ b/source/armarx/navigation/client/services/MemorySubscriber.cpp
@@ -2,7 +2,7 @@
#include <mutex>
#include <type_traits>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
@@ -12,7 +12,7 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include "armarx/navigation/core/aron_conversions.h"
+#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/client/services/MemorySubscriber.h>
#include <armarx/navigation/core/aron/Events.aron.generated.h>
#include <armarx/navigation/core/events.h>
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index 9b77ea8e..c3bec2c6 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -38,8 +38,8 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include "Visu.h"
-#include "armarx/navigation/algorithms/Costmap.h"
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 008ab81c..d47853a7 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -44,24 +44,24 @@
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include "ArmarXCore/core/time/Clock.h"
-#include "ArmarXCore/core/time/Duration.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/time/Clock.h>
+#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
-#include "RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.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>
@@ -69,6 +69,7 @@
#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>
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 3883361f..6ce99900 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -33,38 +33,38 @@
#include <Ice/Object.h>
-#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include "RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h"
-#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
#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>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
-#include "armarx/navigation/memory/client/costmap/Writer.h"
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/components/Navigator/RemoteGui.h>
-#include <armarx/navigation/core/time/ChronoMonotonicTimeServer.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>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
-#include <armarx/navigation/server/introspection/ArvizIntrospector.h>
#include <armarx/navigation/server/execution/PlatformControllerExecutor.h>
+#include <armarx/navigation/server/introspection/ArvizIntrospector.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
#include <armarx/navigation/server/parameterization/MemoryParameterizationService.h>
+#include <armarx/navigation/server/scene_provider/SceneProvider.h>
namespace armarx::navigation::components
diff --git a/source/armarx/navigation/components/Navigator/RemoteGui.cpp b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
index 37a191ba..9ab923ba 100644
--- a/source/armarx/navigation/components/Navigator/RemoteGui.cpp
+++ b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
@@ -16,7 +16,7 @@
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
#include "Navigator.h"
-#include "armarx/navigation/global_planning/SPFA.h"
+#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/factories/NavigationStackFactory.h>
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 9b03d6b0..fa926fb7 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
@@ -33,19 +33,19 @@
#include <SimoxUtility/algorithm/vector.hpp>
#include <SimoxUtility/color/cmaps/colormaps.h>
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
-#include "ArmarXCore/core/time/Clock.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
-#include "RobotAPI/interface/ArViz/Elements.h"
-#include "RobotAPI/libraries/armem_robot/types.h"
-#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h"
-#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
+#include <RobotAPI/interface/ArViz/Elements.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.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/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 headers you only need in function definitions in the .cpp.
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 7f4f9ba4..6094a09e 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
@@ -23,17 +23,17 @@
#pragma once
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/Component.h>
-#include "RobotAPI/libraries/armem/client/forward_declarations.h"
-#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h"
-#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/client.h>
-#include "armarx/navigation/memory/client/costmap/Reader.h"
-#include "armarx/navigation/memory/client/costmap/Writer.h"
+#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/memory/client/costmap/Writer.h>
#include <armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.h>
diff --git a/source/armarx/navigation/core/aron_conversions.cpp b/source/armarx/navigation/core/aron_conversions.cpp
index 0aeaa75b..7b93cdc2 100644
--- a/source/armarx/navigation/core/aron_conversions.cpp
+++ b/source/armarx/navigation/core/aron_conversions.cpp
@@ -3,7 +3,7 @@
#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/core.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/Trajectory.h>
diff --git a/source/armarx/navigation/core/json_conversions.h b/source/armarx/navigation/core/json_conversions.h
index 1d9d7298..7be22d49 100644
--- a/source/armarx/navigation/core/json_conversions.h
+++ b/source/armarx/navigation/core/json_conversions.h
@@ -23,9 +23,9 @@
#include <SimoxUtility/json/json.hpp>
-#include "RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronGeneratedClass.h"
-#include "RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h"
-#include "RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h"
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronGeneratedClass.h>
+#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
namespace armarx
{
diff --git a/source/armarx/navigation/graph/constants.h b/source/armarx/navigation/graph/constants.h
index 011c0c53..2676cc6d 100644
--- a/source/armarx/navigation/graph/constants.h
+++ b/source/armarx/navigation/graph/constants.h
@@ -23,7 +23,7 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::graph
{
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index c8b84a21..8b965d15 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -24,7 +24,7 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include "armarx/navigation/core/Trajectory.h"
+#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/local_planning/core.h>
diff --git a/source/armarx/navigation/location/constants.h b/source/armarx/navigation/location/constants.h
index 5211f27c..5e138e1e 100644
--- a/source/armarx/navigation/location/constants.h
+++ b/source/armarx/navigation/location/constants.h
@@ -23,7 +23,7 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::location
{
diff --git a/source/armarx/navigation/memory/client/costmap/Reader.cpp b/source/armarx/navigation/memory/client/costmap/Reader.cpp
index 17a48dbc..436fd2b9 100644
--- a/source/armarx/navigation/memory/client/costmap/Reader.cpp
+++ b/source/armarx/navigation/memory/client/costmap/Reader.cpp
@@ -16,9 +16,9 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include "armarx/navigation/algorithms/Costmap.h"
-#include "armarx/navigation/algorithms/aron_conversions.h"
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::costmap
{
diff --git a/source/armarx/navigation/memory/client/costmap/Reader.h b/source/armarx/navigation/memory/client/costmap/Reader.h
index 24b02ada..e970a5f2 100644
--- a/source/armarx/navigation/memory/client/costmap/Reader.h
+++ b/source/armarx/navigation/memory/client/costmap/Reader.h
@@ -26,7 +26,7 @@
#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include "armarx/navigation/algorithms/Costmap.h"
+#include <armarx/navigation/algorithms/Costmap.h>
namespace armarx::navigation::memory::client::costmap
{
diff --git a/source/armarx/navigation/memory/client/costmap/Writer.cpp b/source/armarx/navigation/memory/client/costmap/Writer.cpp
index f076fe1c..9fcca394 100644
--- a/source/armarx/navigation/memory/client/costmap/Writer.cpp
+++ b/source/armarx/navigation/memory/client/costmap/Writer.cpp
@@ -1,8 +1,8 @@
#include "Writer.h"
-#include "armarx/navigation/algorithms/aron/Costmap.aron.generated.h"
-#include "armarx/navigation/algorithms/aron_conversions.h"
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::costmap
diff --git a/source/armarx/navigation/memory/client/costmap/Writer.h b/source/armarx/navigation/memory/client/costmap/Writer.h
index 0e44e30d..f80cdc1a 100644
--- a/source/armarx/navigation/memory/client/costmap/Writer.h
+++ b/source/armarx/navigation/memory/client/costmap/Writer.h
@@ -26,7 +26,7 @@
#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
#include <RobotAPI/libraries/armem_vision/types.h>
-#include "armarx/navigation/algorithms/Costmap.h"
+#include <armarx/navigation/algorithms/Costmap.h>
namespace armarx::navigation::memory::client::costmap
{
diff --git a/source/armarx/navigation/memory/client/events/Writer.cpp b/source/armarx/navigation/memory/client/events/Writer.cpp
index 18220950..83009655 100644
--- a/source/armarx/navigation/memory/client/events/Writer.cpp
+++ b/source/armarx/navigation/memory/client/events/Writer.cpp
@@ -5,7 +5,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/core/aron/Events.aron.generated.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/core/constants.h>
diff --git a/source/armarx/navigation/memory/client/parameterization/Reader.cpp b/source/armarx/navigation/memory/client/parameterization/Reader.cpp
index 311a5f4b..e050b325 100644
--- a/source/armarx/navigation/memory/client/parameterization/Reader.cpp
+++ b/source/armarx/navigation/memory/client/parameterization/Reader.cpp
@@ -1,6 +1,6 @@
#include "Reader.h"
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::param
{
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
index 01e7492d..b1fd2da7 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
@@ -1,10 +1,10 @@
#include "PlatformTrajectoryController.h"
-#include "RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
-#include "armarx/navigation/common/controller_types.h"
-#include "armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h"
-#include "armarx/navigation/platform_controller/aron_conversions.h"
+#include <armarx/navigation/common/controller_types.h>
+#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/aron_conversions.h>
#include <armarx/control/common/aron_conversions.h>
#include <armarx/control/common/type.h>
#include <armarx/control/common/utils.h>
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
index a4277f67..9dac5f6b 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
@@ -29,7 +29,7 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
-#include "armarx/navigation/trajectory_control/TrajectoryFollowingController.h"
+#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
#include <armarx/control/common/control_law/ControllerCommonInterface.h>
#include <armarx/control/njoint_controller/task_space/ControllerInterface.h>
#include <armarx/control/njoint_qp_controller/ControllerInterface.h>
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 5581f4d5..c2fab0a1 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -18,6 +18,7 @@
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <VirtualRobot/Robot.h>
+#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -33,12 +34,13 @@
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/events.h>
-#include <armarx/navigation/core/time/TimeServerInterface.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/GlobalPlanner.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>
+
namespace armarx::navigation::server
{
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
index ed6d813e..0882171f 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -1,7 +1,7 @@
#include "PlatformControllerExecutor.h"
-#include "ArmarXCore/core/PackagePath.h"
-#include "ArmarXCore/core/system/ArmarXDataPath.h"
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/control/common/type.h>
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index 4d1dbc1b..0f107c99 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -11,7 +11,7 @@
#include <SimoxUtility/color/ColorMap.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
#include <VirtualRobot/Robot.h>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index 2a68520c..8d71c2cd 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -5,7 +5,7 @@
#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
#include <VirtualRobot/Robot.h>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
--
GitLab
From 10f48ea3e94f7558c4499c22729920932125c492 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 22:54:04 +0200
Subject: [PATCH 27/47] removing time provider
---
source/armarx/navigation/core/CMakeLists.txt | 3 --
.../core/time/ChronoMonotonicTimeServer.cpp | 13 -------
.../core/time/ChronoMonotonicTimeServer.h | 38 -------------------
.../core/time/TimeServerInterface.h | 36 ------------------
source/armarx/navigation/core/types.h | 6 +--
5 files changed, 3 insertions(+), 93 deletions(-)
delete mode 100644 source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp
delete mode 100644 source/armarx/navigation/core/time/ChronoMonotonicTimeServer.h
delete mode 100644 source/armarx/navigation/core/time/TimeServerInterface.h
diff --git a/source/armarx/navigation/core/CMakeLists.txt b/source/armarx/navigation/core/CMakeLists.txt
index 6b56cc43..4ee7e461 100644
--- a/source/armarx/navigation/core/CMakeLists.txt
+++ b/source/armarx/navigation/core/CMakeLists.txt
@@ -17,7 +17,6 @@ armarx_add_library(core
Graph.cpp
aron_conversions.cpp
json_conversions.cpp
- time/ChronoMonotonicTimeServer.cpp
HEADERS
Trajectory.h
constants.h
@@ -35,8 +34,6 @@ armarx_add_library(core
TopologicScene.h
aron_conversions.h
json_conversions.h
- time/ChronoMonotonicTimeServer.h
- time/TimeServerInterface.h
DEPENDENCIES_PUBLIC
ArmarXCoreInterfaces
ArmarXCore
diff --git a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp b/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp
deleted file mode 100644
index fb3db7cb..00000000
--- a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-#include "ChronoMonotonicTimeServer.h"
-
-#include <ArmarXCore/core/time/Clock.h>
-
-namespace armarx::navigation::core
-{
- armarx::core::time::DateTime
- ChronoMonotonicTimeServer::now() const
- {
- return armarx::Clock::Now();
- }
-
-} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.h b/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.h
deleted file mode 100644
index 31d6cf7e..00000000
--- a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/**
- * 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 <armarx/navigation/core/time/TimeServerInterface.h>
-#include <armarx/navigation/core/types.h>
-
-
-namespace armarx::navigation::core
-{
-
- class ChronoMonotonicTimeServer : virtual public TimeServerInterface
- {
- public:
- armarx::core::time::DateTime now() const override;
-
- ~ChronoMonotonicTimeServer() override = default;
- };
-} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/time/TimeServerInterface.h b/source/armarx/navigation/core/time/TimeServerInterface.h
deleted file mode 100644
index fdb74e85..00000000
--- a/source/armarx/navigation/core/time/TimeServerInterface.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/**
- * 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"
-
-namespace armarx::navigation::core
-{
-
- class TimeServerInterface
- {
- public:
- virtual armarx::core::time::DateTime now() const = 0;
-
- virtual ~TimeServerInterface() = default;
- };
-} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/types.h b/source/armarx/navigation/core/types.h
index 3cc08512..7d338dfd 100644
--- a/source/armarx/navigation/core/types.h
+++ b/source/armarx/navigation/core/types.h
@@ -78,14 +78,14 @@ namespace armarx::navigation::core
struct Scene
{
- std::optional<core::StaticScene> staticScene = std::nullopt;
+ DateTime timestamp = DateTime::Invalid();
+ std::optional<core::StaticScene> staticScene = std::nullopt;
std::optional<core::DynamicScene> dynamicScene = std::nullopt;
- // TopologicMapPtr topologicMap;
+
VirtualRobot::RobotPtr robot;
std::optional<core::SceneGraph> graph;
- TimeServerInterface* timeServer;
};
struct PIDParams
--
GitLab
From 1ab7081f43a12063ae515a69bdba2305c84888c4 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 22:54:40 +0200
Subject: [PATCH 28/47] usage of scene provider
---
.../armarx/navigation/server/CMakeLists.txt | 81 ++++++++++---------
source/armarx/navigation/server/Navigator.cpp | 47 ++++++-----
source/armarx/navigation/server/Navigator.h | 9 ++-
.../server/monitoring/GoalReachedMonitor.cpp | 30 +++----
4 files changed, 95 insertions(+), 72 deletions(-)
diff --git a/source/armarx/navigation/server/CMakeLists.txt b/source/armarx/navigation/server/CMakeLists.txt
index a6ec351b..0e08f94b 100644
--- a/source/armarx/navigation/server/CMakeLists.txt
+++ b/source/armarx/navigation/server/CMakeLists.txt
@@ -1,56 +1,65 @@
armarx_add_library(server
SOURCES
- ./Navigator.cpp
- ./StackResult.cpp
- ./GraphBuilder.cpp
+ Navigator.cpp
+ StackResult.cpp
+ GraphBuilder.cpp
# Executors.
# ./execution/PlatformUnitExecutor.cpp
- ./execution/PlatformControllerExecutor.cpp
+ execution/PlatformControllerExecutor.cpp
# Event publishing.
- ./event_publishing/MemoryPublisher.cpp
+ event_publishing/MemoryPublisher.cpp
# Introspection
- ./introspection/ArvizIntrospector.cpp
- ./introspection/MemoryIntrospector.cpp
+ introspection/ArvizIntrospector.cpp
+ introspection/MemoryIntrospector.cpp
# monitoring
- ./monitoring/GoalReachedMonitor.cpp
+ monitoring/GoalReachedMonitor.cpp
# parameterization
- ./parameterization/MemoryParameterizationService.cpp
+ parameterization/MemoryParameterizationService.cpp
+ # scene provider
+ scene_provider/SceneProvider.cpp
HEADERS
- ./Navigator.h
- ./NavigationStack.h
- ./StackResult.h
- ./GraphBuilder.h
+ Navigator.h
+ NavigationStack.h
+ StackResult.h
+ GraphBuilder.h
# Executors.
- ./execution/ExecutorInterface.h
+ execution/ExecutorInterface.h
# ./execution/PlatformUnitExecutor.h
- ./execution/PlatformControllerExecutor.h
- ./execution/DummyExecutor.h
+ execution/PlatformControllerExecutor.h
+ execution/DummyExecutor.h
# Event publishing.
- ./event_publishing/EventPublishingInterface.h
- ./event_publishing/MemoryPublisher.h
+ event_publishing/EventPublishingInterface.h
+ event_publishing/MemoryPublisher.h
# Introspection
- ./introspection/IntrospectorInterface.h
- ./introspection/ArvizIntrospector.h
- ./introspection/MemoryIntrospector.h
+ introspection/IntrospectorInterface.h
+ introspection/ArvizIntrospector.h
+ introspection/MemoryIntrospector.h
# monitoring
- ./monitoring/GoalReachedMonitor.h
+ monitoring/GoalReachedMonitor.h
# parameterization
- ./parameterization/MemoryParameterizationService.h
+ parameterization/MemoryParameterizationService.h
+ # scene provider
+ scene_provider/SceneProviderInterface.h
+ scene_provider/SceneProvider.h
DEPENDENCIES_PUBLIC
- ArmarXCoreInterfaces
- ArmarXCore
- ArViz # RobotAPI
- armarx_navigation::core
- armarx_navigation::util
- armarx_navigation::global_planning
- armarx_navigation::local_planning
- armarx_navigation::trajectory_control
- armarx_navigation::safety_control
- armarx_navigation::memory
- armarx_control::client
- armarx_control::njoint_qp_controller_ice
+ ArmarXCoreInterfaces
+ ArmarXCore
+ # RobotAPI
+ ArViz
+ armem_robot
+ armem_robot_state
+ # armarx_navigation
+ armarx_navigation::core
+ armarx_navigation::util
+ armarx_navigation::global_planning
+ armarx_navigation::local_planning
+ armarx_navigation::trajectory_control
+ armarx_navigation::safety_control
+ armarx_navigation::memory
+ armarx_control::client
+ armarx_control::njoint_qp_controller_ice
DEPENDENCIES_PRIVATE
- range-v3::range-v3
+ range-v3::range-v3
)
armarx_add_test(server_test
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index c2fab0a1..f0d215d5 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -56,7 +56,7 @@ namespace armarx::navigation::server
Navigator::Navigator(const Config& config, const InjectedServices& services) :
config{config}, srv{services}
{
- ARMARX_CHECK_NOT_NULL(config.scene) << "The scene must be set!";
+ 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.publisher) << "The publisher service must be set!";
}
@@ -153,7 +153,7 @@ namespace armarx::navigation::server
std::end(waypoints),
std::back_inserter(globalWaypoints),
[&](const core::Pose& p)
- { return core::Pose(config.scene->robot->getGlobalPose()) * p; });
+ { return core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()) * p; });
break;
}
@@ -430,7 +430,7 @@ namespace armarx::navigation::server
{
//
GraphBuilder graphBuilder;
- graphBuilder.initialize(core::Pose(config.scene->robot->getGlobalPose()));
+ graphBuilder.initialize(core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()));
// std::optional<Graph*> activeSubgraph;
for (const auto& target : targets)
@@ -453,7 +453,7 @@ namespace armarx::navigation::server
// if (not target.locationId->empty())
// {
- // const auto& subgraph = core::getSubgraph(target.locationId.value(),config.scene->graph->subgraphs);
+ // const auto& subgraph = core::getSubgraph(target.locationId.value(),srv.sceneProvider->scene().graph->subgraphs);
// return subgraph.name() != activeSubgraph;
// }
@@ -482,7 +482,7 @@ namespace armarx::navigation::server
if (not target.locationId->empty())
{
const auto& subgraph =
- core::getSubgraph(target.locationId.value(), config.scene->graph->subgraphs);
+ core::getSubgraph(target.locationId.value(), srv.sceneProvider->scene().graph->subgraphs);
const auto vertex = core::getVertexByName(target.locationId.value(), subgraph);
@@ -545,7 +545,7 @@ namespace armarx::navigation::server
goalReachedMonitor = std::nullopt;
goalReachedMonitor =
- GoalReachedMonitor(graphBuilder.goalPose(), *config.scene, GoalReachedMonitorConfig());
+ GoalReachedMonitor(graphBuilder.goalPose(), srv.sceneProvider->scene(), GoalReachedMonitorConfig());
if (goalReachedMonitor->goalReached())
{
@@ -554,8 +554,8 @@ namespace armarx::navigation::server
<< ". Robot won't move.";
srv.publisher->goalReached(
- core::GoalReachedEvent{{config.scene->timeServer->now()},
- core::Pose(config.scene->robot->getGlobalPose())});
+ core::GoalReachedEvent{{armarx::Clock::Now()},
+ core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
return;
}
@@ -574,7 +574,7 @@ namespace armarx::navigation::server
ARMARX_TRACE;
std::vector<core::Pose> vertexPoses;
- vertexPoses.emplace_back(config.scene->robot->getGlobalPose());
+ vertexPoses.emplace_back(srv.sceneProvider->scene().robot->getGlobalPose());
ARMARX_INFO << "Navigating along the following nodes:";
for (const semrel::ShapeID& vertex : shortestPath)
@@ -648,7 +648,7 @@ namespace armarx::navigation::server
// Could be required if pauseMovement() has been called in the past.
resume();
srv.publisher->movementStarted(core::MovementStartedEvent{
- {config.scene->timeServer->now()},core::Pose(config.scene->robot->getGlobalPose())});
+ {armarx::Clock::Now()},core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
}
void
@@ -666,21 +666,21 @@ namespace armarx::navigation::server
ARMARX_TRACE;
ARMARX_CHECK_NOT_EMPTY(waypoints);
- ARMARX_INFO << "Request to move from " << config.scene->robot->getGlobalPose() << " to "
+ ARMARX_INFO << "Request to move from " << srv.sceneProvider->scene().robot->getGlobalPose() << " to "
<< waypoints.back().matrix();
// first we check if we are already at the goal position
goalReachedMonitor = std::nullopt;
goalReachedMonitor =
- GoalReachedMonitor(waypoints.back(), *config.scene, GoalReachedMonitorConfig());
+ 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{{config.scene->timeServer->now()},
- core::Pose(config.scene->robot->getGlobalPose())});
+ core::GoalReachedEvent{{armarx::Clock::Now()},
+ core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
return;
}
@@ -700,7 +700,7 @@ namespace armarx::navigation::server
{
ARMARX_WARNING << "No global trajectory. Cannot move.";
srv.publisher->globalPlanningFailed(core::GlobalPlanningFailedEvent{
- {.timestamp = config.scene->timeServer->now()}, {""}});
+ {.timestamp = armarx::Clock::Now()}, {""}});
srv.introspector->failure();
return;
@@ -743,7 +743,7 @@ namespace armarx::navigation::server
// TODO check if startPose shouldn't be waypoints.front() etc
srv.publisher->movementStarted(core::MovementStartedEvent{
- {config.scene->timeServer->now()}, {waypoints.back()}});
+ {armarx::Clock::Now()}, {waypoints.back()}});
ARMARX_INFO << "Movement started.";
}
@@ -757,6 +757,7 @@ namespace armarx::navigation::server
Navigator::moveTowardsAbsolute(const core::Direction& direction)
{
}
+
void
Navigator::replan()
@@ -766,6 +767,12 @@ namespace armarx::navigation::server
updateExecutor();
updateIntrospector();
}
+
+ void Navigator::updateScene()
+ {
+ ARMARX_CHECK_NOT_NULL(srv.sceneProvider);
+ srv.sceneProvider->synchronize(armarx::Clock::Now());
+ }
void
Navigator::updateLocalPlanner()
@@ -800,11 +807,11 @@ namespace armarx::navigation::server
}
- // const core::Rotation robot_R_world(config.scene->robot->getGlobalOrientation().inverse());
+ // const core::Rotation robot_R_world(srv.sceneProvider->scene().robot->getGlobalOrientation().inverse());
// ARMARX_VERBOSE
// << deactivateSpam(100) << "Robot orientation "
- // << simox::math::mat3f_to_rpy(config.scene->robot->getGlobalOrientation()).z();
+ // << simox::math::mat3f_to_rpy(srv.sceneProvider->scene().robot->getGlobalOrientation()).z();
// core::Twist robotFrameVelocity;
@@ -852,8 +859,8 @@ namespace armarx::navigation::server
stop();
srv.publisher->goalReached(
- core::GoalReachedEvent{{config.scene->timeServer->now()},
- {core::Pose(config.scene->robot->getGlobalPose())}});
+ core::GoalReachedEvent{{armarx::Clock::Now()},
+ {core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())}});
srv.introspector->success();
diff --git a/source/armarx/navigation/server/Navigator.h b/source/armarx/navigation/server/Navigator.h
index 933a1c24..19318c01 100644
--- a/source/armarx/navigation/server/Navigator.h
+++ b/source/armarx/navigation/server/Navigator.h
@@ -52,6 +52,11 @@
namespace armarx::navigation::server
{
+ namespace scene_provider
+ {
+ class SceneProviderInterface;
+ }
+
class Navigator : virtual public core::NavigatorInterface
{
@@ -68,7 +73,6 @@ namespace armarx::navigation::server
};
server::NavigationStack stack;
- core::Scene* const scene;
General general;
};
@@ -77,6 +81,8 @@ namespace armarx::navigation::server
ExecutorInterface* executor;
EventPublishingInterface* publisher;
IntrospectorInterface* introspector = nullptr;
+
+ scene_provider::SceneProviderInterface* sceneProvider;
};
Navigator(const Config& config, const InjectedServices& services);
@@ -113,6 +119,7 @@ namespace armarx::navigation::server
void replan();
+ void updateScene();
void updateLocalPlanner();
void updateExecutor();
void updateIntrospector();
diff --git a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp
index 923eeb12..7962ed8e 100644
--- a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp
+++ b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp
@@ -42,25 +42,25 @@ namespace armarx::navigation::server
ARMARX_DEBUG << "Orientation goal reached";
}
+ // FIXME:
+ // if (not scene.platformVelocity.has_value())
+ // {
+ // ARMARX_DEBUG << "Platform velocity not available yet.";
+ // return false;
+ // }
- if (not scene.platformVelocity.has_value())
- {
- ARMARX_DEBUG << "Platform velocity not available yet.";
- return false;
- }
-
- const bool linearVelocityLow = scene.platformVelocity->linear.norm() < config.linearVelTh;
- const bool angularVelocityLow =
- scene.platformVelocity->angular.norm() < config.angularVelTh;
+ // const bool linearVelocityLow = scene.platformVelocity->linear.norm() < config.linearVelTh;
+ // const bool angularVelocityLow =
+ // scene.platformVelocity->angular.norm() < config.angularVelTh;
- const bool velocityLow = linearVelocityLow and angularVelocityLow;
+ // const bool velocityLow = linearVelocityLow and angularVelocityLow;
- if (velocityLow)
- {
- ARMARX_DEBUG << "Robot has come to rest.";
- }
+ // if (velocityLow)
+ // {
+ // ARMARX_DEBUG << "Robot has come to rest.";
+ // }
- return posReached and oriReached and velocityLow;
+ return posReached and oriReached; //and velocityLow;
}
const core::Pose&
--
GitLab
From 04c4f972ddc72ec6a0dc060bc25cf738cf0cb13f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 22:54:56 +0200
Subject: [PATCH 29/47] navigator component: cleanup : moved scene update logic
into separate class
---
.../components/Navigator/Navigator.cpp | 336 ++++++++----------
.../components/Navigator/Navigator.h | 48 +--
2 files changed, 178 insertions(+), 206 deletions(-)
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index d47853a7..26ae33d2 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -118,7 +118,7 @@ namespace armarx::navigation::components
components::Navigator::Navigator() : parameterizationService(nullptr, nullptr)
// publisher(&resultsWriter, &eventsWriter)
{
- // scene.timeServer = &timeServer;
+ // scene().timeServer = &timeServer;
addPlugin(parameterizationReaderPlugin);
addPlugin(parameterizationWriterPlugin);
@@ -126,7 +126,7 @@ namespace armarx::navigation::components
addPlugin(resultsWriterPlugin);
addPlugin(graphReaderPlugin);
addPlugin(costmapWriterPlugin);
-
+
addPlugin(virtualRobotReaderPlugin);
parameterizationService = server::MemoryParameterizationService(
@@ -160,25 +160,31 @@ namespace armarx::navigation::components
virtualRobotReaderPlugin->get().setSyncTimeout(Duration::MilliSeconds(20));
virtualRobotReaderPlugin->get().setSleepAfterSyncFailure(Duration::MilliSeconds(10));
- // initialize scene
+ // initialize scene provider
ARMARX_TRACE;
+ {
+ const server::scene_provider::SceneProvider::InjectedServices srv{
+ .graphReader = &graphReaderPlugin->get(),
+ .costmapReader = &costmapReaderPlugin->get(),
+ .virtualRobotReader = &virtualRobotReaderPlugin->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);
+ }
+
initializeScene();
executor.emplace(server::PlatformControllerExecutor(getControlComponentPlugin()));
- introspector = server::ArvizIntrospector(getArvizClient(), scene.robot);
+ introspector = server::ArvizIntrospector(getArvizClient(), scene().robot);
- // TODO dynamic scene
- // TODO memory
- // TODO param (10)
-
-
- // robotStateUpdateTask = new PeriodicTask<Navigator>(this,
- // &Navigator::updateRobot,
- // params.robotUpdatePeriodMs,
- // false,
- // "RobotStateUpdateTask");
- // robotStateUpdateTask->start();
navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
navRemoteGui->enable();
@@ -192,12 +198,6 @@ namespace armarx::navigation::components
components::Navigator::onReconnectComponent()
{
ARMARX_TRACE;
-
- robotStateUpdateTask->start();
-
- // TODO not in all cases meaningful
- //resume();
-
navRemoteGui->enable();
}
@@ -206,10 +206,7 @@ namespace armarx::navigation::components
{
ARMARX_TRACE;
- robotStateUpdateTask->stop();
-
stopAll();
-
navRemoteGui->disable();
}
@@ -218,41 +215,28 @@ namespace armarx::navigation::components
{
}
- VirtualRobot::RobotPtr
- components::Navigator::getRobot()
- {
- ARMARX_TRACE;
- auto robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(
- getControlComponentPlugin().getRobotUnitPlugin().getRobotUnit()->getKinematicUnit()->getRobotName(),
- VirtualRobot::RobotIO::RobotDescription::eCollisionModel,
- true);
-
- ARMARX_CHECK_NOT_NULL(robot);
- return robot;
- }
+ // VirtualRobot::RobotPtr
+ // components::Navigator::getRobot()
+ // {
+ // ARMARX_TRACE;
+ // auto robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(
+ // getControlComponentPlugin()
+ // .getRobotUnitPlugin()
+ // .getRobotUnit()
+ // ->getKinematicUnit()
+ // ->getRobotName(),
+ // VirtualRobot::RobotIO::RobotDescription::eCollisionModel,
+ // true);
+
+ // ARMARX_CHECK_NOT_NULL(robot);
+ // return robot;
+ // }
void
components::Navigator::initializeScene()
{
ARMARX_TRACE;
-
- scene.robot = getRobot();
-
- {
- if (not scene.staticScene.has_value())
- {
- scene.staticScene = staticScene();
-
- // cv::Mat1f mat;
- // cv::eigen2cv(scene.staticScene->costmap->getGrid(), mat);
- // cv::imwrite("/tmp/grid.exr", mat);
- }
- }
-
- // TODO dynamic scene
- {
- scene.graph = core::SceneGraph{.subgraphs = graphReaderPlugin->get().graphs()};
- }
+ sceneProvider->initialize(armarx::Clock::Now());
}
std::string
@@ -270,9 +254,10 @@ namespace armarx::navigation::components
ARMARX_TRACE;
ARMARX_INFO << "Creating config for caller '" << callerId << "'";
- parameterizationService.store(stackConfig, callerId, timeServer.now());
+ parameterizationService.store(stackConfig, callerId, armarx::Clock::Now());
- server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
+ server::NavigationStack stack =
+ fac::NavigationStackFactory::create(stackConfig, sceneProvider->scene());
memoryIntrospectors.emplace_back(
std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
@@ -288,12 +273,12 @@ namespace armarx::navigation::components
std::forward_as_tuple(callerId),
std::forward_as_tuple(
server::Navigator::Config{.stack = std::move(stack),
- .scene = &scene,
.general = server::Navigator::Config::General{}},
server::Navigator::InjectedServices{.executor = &executor.value(),
.publisher =
memoryPublishers.at(callerId).get(),
- .introspector = &(introspector.value())}));
+ .introspector = &(introspector.value()),
+ .sceneProvider = &sceneProvider.value()}));
}
void
@@ -341,7 +326,6 @@ namespace armarx::navigation::components
ARMARX_CHECK(navigators.count(callerId) > 0)
<< "Navigator config for caller `" << callerId << "` not registered!";
- visualizeSPFA();
navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
}
@@ -389,8 +373,8 @@ namespace armarx::navigation::components
navigators.at(configId).stop();
// emit UserAbortTriggered event
- const core::UserAbortTriggeredEvent event{{timeServer.now()},
- core::Pose(scene.robot->getGlobalPose())};
+ const core::UserAbortTriggeredEvent event{{armarx::Clock::Now()},
+ core::Pose(scene().robot->getGlobalPose())};
memoryPublishers.at(configId)->userAbortTriggered(event);
}
@@ -406,8 +390,8 @@ namespace armarx::navigation::components
}
// emit UserAbortTriggered event
- const core::UserAbortTriggeredEvent event{{timeServer.now()},
- core::Pose(scene.robot->getGlobalPose())};
+ const core::UserAbortTriggeredEvent event{{armarx::Clock::Now()},
+ core::Pose(scene().robot->getGlobalPose())};
for (auto& [callerId, memoryPublisher] : memoryPublishers)
{
memoryPublisher->userAbortTriggered(event);
@@ -423,6 +407,13 @@ namespace armarx::navigation::components
return navigators.at(configId).isPaused();
}
+ const core::Scene&
+ Navigator::scene() const
+ {
+ ARMARX_CHECK_NOT_NULL(sceneProvider);
+ return sceneProvider->scene();
+ }
+
bool
components::Navigator::isStopped(const std::string& configId, const Ice::Current&)
{
@@ -500,159 +491,140 @@ namespace armarx::navigation::components
arviz.commit(layer);
}
- core::StaticScene
- components::Navigator::staticScene()
- {
- ARMARX_TRACE;
+ // core::StaticScene
+ // components::Navigator::staticScene()
+ // {
+ // ARMARX_TRACE;
- const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
+ // 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"});
+ // // 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";
+ // 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);
+ // 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");
+ // algorithms::CostmapBuilder costmapBuilder(
+ // scene().robot,
+ // objects,
+ // algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
+ // "Platform-navigation-colmodel");
- const auto costmap = costmapBuilder.create();
+ // const auto costmap = costmapBuilder.create();
- ARMARX_INFO << "Storing costmap in memory";
- costmapWriterPlugin->get().store(
- costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
+ // ARMARX_INFO << "Storing costmap in memory";
+ // costmapWriterPlugin->get().store(
+ // costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
- ARMARX_INFO << "Done";
+ // ARMARX_INFO << "Done";
- ARMARX_TRACE;
- const auto grid = costmap.getGrid();
+ // ARMARX_TRACE;
+ // const auto grid = costmap.getGrid();
- core::StaticScene scene{.objects = objects,
- .costmap = std::make_unique<algorithms::Costmap>(costmap)};
+ // core::StaticScene scene{.objects = objects,
+ // .costmap = std::make_unique<algorithms::Costmap>(costmap)};
- ARMARX_INFO << "The object scene consists of " << scene.objects->getSize() << " objects";
+ // ARMARX_INFO << "The object scene consists of " << scene().objects->getSize() << " objects";
- visualize(costmap, arviz, "distance");
+ // // visualize(costmap, arviz, "distance");
- /*
- const armem::vision::occupancy_grid::client::Reader::Result result =
- occupancyGridReader.query(armem::vision::occupancy_grid::client::Reader::Query{
- .providerName = "CartographerMappingAndLocalization",
- .timestamp = armem::Time::Now()});
+ // /*
+ // const armem::vision::occupancy_grid::client::Reader::Result result =
+ // occupancyGridReader.query(armem::vision::occupancy_grid::client::Reader::Query{
+ // .providerName = "CartographerMappingAndLocalization",
+ // .timestamp = armem::Time::Now()});
- if (result and result.occupancyGrid.has_value() and false) // Feature disabled on master ...
- {
- ARMARX_INFO << "Occupancy grid available!";
+ // if (result and result.occupancyGrid.has_value() and false) // Feature disabled on master ...
+ // {
+ // ARMARX_INFO << "Occupancy grid available!";
- const auto occupancyGridSceneElements = util::asSceneObjects(
- result.occupancyGrid.value(),
- OccupancyGridHelper::Params{.freespaceThreshold = 0.45F,
- .occupiedThreshold = params.occupiedGridThreshold});
- ARMARX_INFO << occupancyGridSceneElements->getSize()
- << " scene elements from occupancy grid";
+ // const auto occupancyGridSceneElements = util::asSceneObjects(
+ // result.occupancyGrid.value(),
+ // OccupancyGridHelper::Params{.freespaceThreshold = 0.45F,
+ // .occupiedThreshold = params.occupiedGridThreshold});
+ // ARMARX_INFO << occupancyGridSceneElements->getSize()
+ // << " scene elements from occupancy grid";
- scene.objects->addSceneObjects(occupancyGridSceneElements);
+ // scene().objects->addSceneObjects(occupancyGridSceneElements);
- // draw
- auto layer = arviz.layer("occupancy_grid");
+ // // 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()));
- }
+ // 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";
+ // ARMARX_INFO << "Creating costmap";
- algorithms::CostmapBuilder costmapBuilder(
- getRobot(),
- scene.objects,
- algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
- "Platform-navigation-colmodel");
+ // algorithms::CostmapBuilder costmapBuilder(
+ // getRobot(),
+ // scene().objects,
+ // algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
+ // "Platform-navigation-colmodel");
- const auto costmap = costmapBuilder.create();
+ // const auto costmap = costmapBuilder.create();
- ARMARX_INFO << "Done";
+ // ARMARX_INFO << "Done";
- ARMARX_TRACE;
- ARMARX_INFO << "Saving costmap.";
- algorithms::save(costmap, "/tmp/navigation-costmap");
+ // ARMARX_TRACE;
+ // ARMARX_INFO << "Saving costmap.";
+ // algorithms::save(costmap, "/tmp/navigation-costmap");
- arviz.commit({layer});
- }
- else
- {
- ARMARX_INFO << "Occupancy grid not available";
- }*/
+ // arviz.commit({layer});
+ // }
+ // else
+ // {
+ // ARMARX_INFO << "Occupancy grid not available";
+ // }*/
- return scene;
- }
+ // return scene;
+ // }
- void
- components::Navigator::visualizeSPFA()
- {
- algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams{
- .obstacleDistanceCosts = true};
- ARMARX_INFO << "spfa...";
- algorithms::spfa::ShortestPathFasterAlgorithm spfa(*scene.staticScene->costmap, spfaParams);
- const auto result123 = spfa.spfa(getRobot()->getGlobalPosition().head<2>());
- ARMARX_INFO << "spfa done...";
+ // void
+ // components::Navigator::updateRobot()
+ // {
+ // const auto timestamp = armem::Time::Now();
- const auto sceneBounds = algorithms::computeSceneBounds(scene.staticScene->objects);
+ // auto& virtualRobotReader = virtualRobotReaderPlugin->get();
- ARMARX_INFO << "Costmap SPFA";
- algorithms::Costmap cm(result123.distances, algorithms::Costmap::Parameters{}, sceneBounds);
- visualize(cm, arviz, "spfa");
- ARMARX_INFO << "Done.";
- }
+ // ARMARX_TRACE;
+ // ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*scene().robot, timestamp));
+ // ARMARX_TRACE;
+ // const auto robotDescription =
+ // virtualRobotReader.queryDescription(scene().robot->getType(), timestamp);
+ // ARMARX_CHECK(robotDescription.has_value());
- void
- components::Navigator::updateRobot()
- {
- const auto timestamp = armem::Time::Now();
-
- auto& virtualRobotReader = virtualRobotReaderPlugin->get();
-
- ARMARX_TRACE;
- ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*scene.robot, timestamp));
+ // // synchronizing platform state
+ // ARMARX_TRACE;
+ // const std::optional<armem::robot::PlatformState> platformState =
+ // virtualRobotReader.queryPlatformState(robotDescription.value(), armem::Time::Now());
+ // ARMARX_CHECK(platformState.has_value());
- ARMARX_TRACE;
- const auto robotDescription =
- virtualRobotReader.queryDescription(scene.robot->getType(), 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!
+ // {
+ // if (not scene().platformVelocity.has_value())
+ // {
+ // scene().platformVelocity = core::Twist();
+ // }
- // TODO / FIXME make this thread safe!
- {
- if (not scene.platformVelocity.has_value())
- {
- scene.platformVelocity = core::Twist();
- }
-
- scene.platformVelocity->linear = platformState->twist.linear;
- scene.platformVelocity->angular = platformState->twist.angular;
- }
- }
+ // scene().platformVelocity->linear = platformState->twist.linear;
+ // scene().platformVelocity->angular = platformState->twist.angular;
+ // }
+ // }
server::Navigator*
components::Navigator::activeNavigator()
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 6ce99900..d1f37ccc 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -87,7 +87,7 @@ namespace armarx::navigation::components
virtual public ObjectPoseClientPluginUser,
virtual public ArVizComponentPluginUser,
virtual public armarx::control::client::ComponentPluginUser
- // virtual public armem::ListeningClientPluginUser
+ // virtual public armem::ListeningClientPluginUser
{
public:
@@ -131,11 +131,7 @@ namespace armarx::navigation::components
bool isStopped(const std::string& callerId,
const Ice::Current& c = Ice::emptyCurrent) override;
- const core::Scene&
- getScene() const
- {
- return scene;
- }
+ const core::Scene& scene() const;
protected:
/// @see PropertyUser::createPropertyDefinitions()
@@ -156,10 +152,10 @@ namespace armarx::navigation::components
void onReconnectComponent();
void initializeScene();
- core::StaticScene staticScene();
+ // core::StaticScene staticScene();
- [[nodiscard]] VirtualRobot::RobotPtr getRobot();
- void updateRobot();
+ // [[nodiscard]] VirtualRobot::RobotPtr getRobot();
+ // void updateRobot();
server::Navigator* activeNavigator();
@@ -168,16 +164,16 @@ namespace armarx::navigation::components
RemoteGuiInterfacePrx remoteGui;
- core::Scene scene;
+ // core::Scene scene;
std::optional<server::PlatformControllerExecutor> executor;
std::optional<server::ArvizIntrospector> introspector;
+ std::optional<server::scene_provider::SceneProvider> sceneProvider;
+
std::unordered_map<std::string, server::Navigator> navigators;
std::mutex propertiesMutex;
- PeriodicTask<Navigator>::pointer_type robotStateUpdateTask;
-
// TODO maybe as optional, but requires some effort
std::unique_ptr<NavigatorRemoteGui> navRemoteGui;
@@ -186,17 +182,26 @@ namespace armarx::navigation::components
std::vector<std::unique_ptr<server::MemoryIntrospector>> memoryIntrospectors;
// `navigation` memory reader and writer
- armem::client::plugins::ReaderWriterPlugin<memory::client::param::Reader>* parameterizationReaderPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::param::Writer>* parameterizationWriterPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::events::Writer>* eventsWriterPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::stack_result::Writer>* resultsWriterPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::graph::Reader>* graphReaderPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Writer>* costmapWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::param::Reader>*
+ parameterizationReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::param::Writer>*
+ parameterizationWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::events::Writer>*
+ eventsWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::stack_result::Writer>*
+ resultsWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::graph::Reader>*
+ graphReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Reader>*
+ costmapReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Writer>*
+ costmapWriterPlugin = nullptr;
// armem::vision::occupancy_grid::client::Reader occupancyGridReader;
// `robot_state` memory reader and writer
std::optional<armem::robot::RobotDescription> robotDescription;
- armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>*
+ virtualRobotReaderPlugin = nullptr;
server::MemoryParameterizationService parameterizationService;
@@ -205,14 +210,9 @@ namespace armarx::navigation::components
// key is `callerId`
std::unordered_map<std::string, std::unique_ptr<server::MemoryPublisher>> memoryPublishers;
- core::ChronoMonotonicTimeServer timeServer;
-
-
struct Parameters
{
float occupiedGridThreshold{0.55F};
-
- int robotUpdatePeriodMs = 10;
};
Parameters params;
--
GitLab
From b2c66dc8d4c3157e5bb28e54736cc6f2b8f04c40 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 13 Jul 2022 13:27:54 +0200
Subject: [PATCH 30/47] minor
---
.../trajectory_control/TrajectoryFollowingController.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index 8d71c2cd..486ba3d0 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -5,8 +5,8 @@
#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
#include <VirtualRobot/Robot.h>
-#include <ArmarXCore/core/logging/Logging.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>
--
GitLab
From c0840fbf65a46b5f0cac9d2bc9418ca3e025f39e Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 13 Jul 2022 13:28:03 +0200
Subject: [PATCH 31/47] TrajectoryFollowingController: floating point rounding
fix
---
.../TrajectoryFollowingController.cpp | 14 ++++++++------
1 file changed, 8 insertions(+), 6 deletions(-)
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index 486ba3d0..b5ef2b45 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -112,13 +112,15 @@ namespace armarx::navigation::traj_ctrl
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);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular);
+ 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;
}
--
GitLab
From e720b9cff920be0b255cd6a1be7cf63a1c3efc84 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 13 Jul 2022 13:28:33 +0200
Subject: [PATCH 32/47] minor
---
.../navigation/server/scene_provider/SceneProvider.cpp | 6 ++++--
1 file changed, 4 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
index 04412bb3..804c4d03 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
@@ -1,6 +1,7 @@
#include "SceneProvider.h"
#include <VirtualRobot/SceneObjectSet.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/util/util.h>
@@ -25,7 +26,8 @@ namespace armarx::navigation::server::scene_provider
scn.timestamp = timestamp;
scn.robot = srv.virtualRobotReader->getRobot(
- config.robotName, timestamp, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ config.robotName, timestamp, VirtualRobot::RobotIO::RobotDescription::eCollisionModel);
+ ARMARX_CHECK_NOT_NULL(scn.robot);
scn.staticScene = getStaticScene(timestamp);
scn.dynamicScene = getDynamicScene(timestamp);
@@ -43,7 +45,7 @@ namespace armarx::navigation::server::scene_provider
srv.virtualRobotReader->synchronizeRobot(*scn.robot, timestamp);
scn.dynamicScene = getDynamicScene(timestamp);
- scn.graph = getSceneGraph(timestamp);
+ // scn.graph = getSceneGraph(timestamp);
return true; // TODO(fabian.reister): return false if sync fails
}
--
GitLab
From 5b0ecd9af6284b0dbdd92ad8f0fb6266610b5c3e Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 13 Jul 2022 13:29:14 +0200
Subject: [PATCH 33/47] navigator: simplification
---
source/armarx/navigation/server/Navigator.cpp | 287 +++++++++---------
source/armarx/navigation/server/Navigator.h | 23 +-
.../execution/PlatformControllerExecutor.cpp | 32 +-
3 files changed, 177 insertions(+), 165 deletions(-)
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index f0d215d5..2be9420b 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -18,12 +18,14 @@
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <VirtualRobot/Robot.h>
-#include <ArmarXCore/core/time/Clock.h>
+#include "ArmarXCore/core/time/StopWatch.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include "armarx/navigation/local_planning/LocalPlanner.h"
#include "range/v3/algorithm/reverse.hpp"
#include "range/v3/range/conversion.hpp"
#include <SemanticObjectRelations/Shapes/Shape.h>
@@ -149,11 +151,12 @@ namespace armarx::navigation::server
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 core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()) * p; });
+ std::transform(
+ std::begin(waypoints),
+ std::end(waypoints),
+ std::back_inserter(globalWaypoints),
+ [&](const core::Pose& p)
+ { return core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()) * p; });
break;
}
@@ -383,7 +386,7 @@ namespace armarx::navigation::server
.velocity = 0};
// resample event straight lines
-// ARMARX_CHECK_NOT_EMPTY(trajectoryPoints);
+ // ARMARX_CHECK_NOT_EMPTY(trajectoryPoints);
// const core::Trajectory segmentTraj({trajectoryPoints.back(), nextTrajPt});
// ARMARX_INFO << "Segment trajectory length: " << segmentTraj.length();
@@ -481,8 +484,8 @@ namespace armarx::navigation::server
// instead. Thus, we collect all routes to reach the node.
if (not target.locationId->empty())
{
- const auto& subgraph =
- core::getSubgraph(target.locationId.value(), srv.sceneProvider->scene().graph->subgraphs);
+ const auto& subgraph = core::getSubgraph(
+ target.locationId.value(), srv.sceneProvider->scene().graph->subgraphs);
const auto vertex = core::getVertexByName(target.locationId.value(), subgraph);
@@ -544,8 +547,8 @@ namespace armarx::navigation::server
ARMARX_TRACE;
goalReachedMonitor = std::nullopt;
- goalReachedMonitor =
- GoalReachedMonitor(graphBuilder.goalPose(), srv.sceneProvider->scene(), GoalReachedMonitorConfig());
+ goalReachedMonitor = GoalReachedMonitor(
+ graphBuilder.goalPose(), srv.sceneProvider->scene(), GoalReachedMonitorConfig());
if (goalReachedMonitor->goalReached())
{
@@ -553,9 +556,9 @@ namespace armarx::navigation::server
<< goalReachedMonitor->goal().translation().head<2>()
<< ". Robot won't move.";
- srv.publisher->goalReached(
- core::GoalReachedEvent{{armarx::Clock::Now()},
- core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
+ srv.publisher->goalReached(core::GoalReachedEvent{
+ {armarx::Clock::Now()},
+ core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
return;
}
@@ -602,12 +605,12 @@ namespace armarx::navigation::server
// the following is similar to moveToAbsolute
// TODO(fabian.reister): remove code duplication
- ARMARX_TRACE;
+ srv.executor->execute(globalPlan->trajectory);
+ ARMARX_TRACE;
srv.publisher->globalTrajectoryUpdated(globalPlan.value());
ARMARX_TRACE;
-
srv.introspector->onGlobalPlannerResult(globalPlan.value());
ARMARX_INFO << "Global planning completed. Will now start all required threads";
@@ -620,35 +623,21 @@ namespace armarx::navigation::server
Navigator::startStack()
{
- // local planner
- if (config.stack.localPlanner)
- {
- ARMARX_TRACE;
-
- ARMARX_INFO << "Planning local trajectory enabled";
- replanningTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::replan,
- config.general.tasks.replanningUpdatePeriod,
- false,
- "LocalPlannerTask");
- replanningTask->start();
- }
-
+ ARMARX_TRACE;
- // monitoring
- ARMARX_INFO << "Starting monitoring";
- monitorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateMonitor,
- config.general.tasks.monitorUpdatePeriod,
+ ARMARX_INFO << "Planning local trajectory enabled";
+ runningTask = new PeriodicTask<Navigator>(this,
+ &Navigator::run,
+ config.general.tasks.replanningUpdatePeriod,
false,
- "MonitorTask");
- monitorTask->start();
+ "LocalPlannerTask");
+ runningTask->start();
+
// Could be required if pauseMovement() has been called in the past.
resume();
srv.publisher->movementStarted(core::MovementStartedEvent{
- {armarx::Clock::Now()},core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
+ {armarx::Clock::Now()}, core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
}
void
@@ -666,21 +655,21 @@ namespace armarx::navigation::server
ARMARX_TRACE;
ARMARX_CHECK_NOT_EMPTY(waypoints);
- ARMARX_INFO << "Request to move from " << srv.sceneProvider->scene().robot->getGlobalPose() << " to "
- << waypoints.back().matrix();
+ ARMARX_INFO << "Request to move from " << srv.sceneProvider->scene().robot->getGlobalPose()
+ << " to " << waypoints.back().matrix();
// first we check if we are already at the goal position
goalReachedMonitor = std::nullopt;
- goalReachedMonitor =
- GoalReachedMonitor(waypoints.back(), srv.sceneProvider->scene(), GoalReachedMonitorConfig());
+ 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())});
+ srv.publisher->goalReached(core::GoalReachedEvent{
+ {armarx::Clock::Now()},
+ core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
return;
}
@@ -699,51 +688,22 @@ namespace armarx::navigation::server
if (not globalPlan.has_value())
{
ARMARX_WARNING << "No global trajectory. Cannot move.";
- srv.publisher->globalPlanningFailed(core::GlobalPlanningFailedEvent{
- {.timestamp = armarx::Clock::Now()}, {""}});
+ srv.publisher->globalPlanningFailed(
+ core::GlobalPlanningFailedEvent{{.timestamp = armarx::Clock::Now()}, {""}});
srv.introspector->failure();
return;
}
- ARMARX_TRACE;
+ 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;
- // local planner
- if (config.stack.localPlanner)
- {
- ARMARX_INFO << "Planning local trajectory enabled";
- replanningTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::replan,
- config.general.tasks.replanningUpdatePeriod,
- false,
- "LocalPlannerTask");
- replanningTask->start();
- }
-
- // monitoring
- ARMARX_INFO << "Starting monitoring";
- monitorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateMonitor,
- config.general.tasks.monitorUpdatePeriod,
- false,
- "MonitorTask");
- monitorTask->start();
-
- // Could be required if pauseMovement() has been called in the past.
- ARMARX_INFO << "Resuming ...";
- ARMARX_TRACE;
- resume();
- ARMARX_INFO << "Done.";
-
- // TODO check if startPose shouldn't be waypoints.front() etc
- srv.publisher->movementStarted(core::MovementStartedEvent{
- {armarx::Clock::Now()}, {waypoints.back()}});
+ startStack();
ARMARX_INFO << "Movement started.";
}
@@ -757,26 +717,60 @@ namespace armarx::navigation::server
Navigator::moveTowardsAbsolute(const core::Direction& direction)
{
}
-
+
void
- Navigator::replan()
+ Navigator::run()
{
- updateScene();
- updateLocalPlanner();
- updateExecutor();
- updateIntrospector();
+ // TODO(fabian.reister): add debug observer logging
+
+ // scene update
+ {
+ const Duration duration =
+ armarx::core::time::StopWatch::measure([&]() { updateScene(); });
+
+ ARMARX_VERBOSE << deactivateSpam(0.2) << "Scene update: " << duration.toSecondsDouble()
+ << "s.";
+ }
+
+ // local planner update
+ {
+ const Duration duration = armarx::core::time::StopWatch::measure(
+ [&]()
+ {
+ if (hasLocalPlanner())
+ {
+ const auto localPlannerResult = updateLocalPlanner();
+ updateExecutor(localPlannerResult);
+ updateIntrospector(localPlannerResult);
+ }
+ });
+ ARMARX_VERBOSE << deactivateSpam(0.2)
+ << "Local planner update: " << duration.toSecondsDouble() << "s.";
+ }
+
+ // monitor update
+ {
+ const Duration duration =
+ armarx::core::time::StopWatch::measure([&]() { updateMonitor(); });
+
+ ARMARX_VERBOSE << deactivateSpam(0.2)
+ << "Monitor update: " << duration.toSecondsDouble() << "s.";
+ }
}
-
- void Navigator::updateScene()
+
+ void
+ Navigator::updateScene()
{
ARMARX_CHECK_NOT_NULL(srv.sceneProvider);
srv.sceneProvider->synchronize(armarx::Clock::Now());
}
- void
+ std::optional<loc_plan::LocalPlannerResult>
Navigator::updateLocalPlanner()
{
+ ARMARX_CHECK(hasLocalPlanner());
+
localPlan = config.stack.localPlanner->plan(globalPlan->trajectory);
if (localPlan.has_value())
@@ -787,10 +781,12 @@ namespace armarx::navigation::server
{
// srv.publisher->localTrajectoryPlanningFailed();
}
+
+ return localPlan;
}
void
- Navigator::updateExecutor()
+ Navigator::updateExecutor(const std::optional<loc_plan::LocalPlannerResult>& localPlan)
{
if (isPaused() or isStopped())
{
@@ -799,11 +795,10 @@ namespace armarx::navigation::server
return;
}
- if (not isStackResultValid())
+ if (not localPlan.has_value())
{
- ARMARX_VERBOSE << deactivateSpam(1) << "stack result invalid";
- // [[unlikely]]
- return;
+ ARMARX_INFO << "Local plan is invalid!";
+ srv.executor->stop();
}
@@ -822,25 +817,20 @@ namespace armarx::navigation::server
// ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
// << robotFrameVelocity.linear;
- srv.executor->execute(currentTrajectory());
+ srv.executor->execute(localPlan->trajectory);
}
void
- Navigator::updateIntrospector()
+ Navigator::updateIntrospector(const std::optional<loc_plan::LocalPlannerResult>& localPlan)
{
ARMARX_CHECK_NOT_NULL(srv.introspector);
- // TODO event driven. should be committed only on change
- // if(globalPlan.has_value())
- // {
- // srv.introspector->onGlobalPlannerResult(globalPlan.value());
- // }
-
- if (localPlan.has_value())
+ if (not localPlan.has_value())
{
- srv.introspector->onLocalPlannerResult(localPlan.value());
+ return;
}
+ srv.introspector->onLocalPlannerResult(localPlan.value());
}
void
@@ -858,9 +848,9 @@ namespace armarx::navigation::server
stop();
- srv.publisher->goalReached(
- core::GoalReachedEvent{{armarx::Clock::Now()},
- {core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())}});
+ srv.publisher->goalReached(core::GoalReachedEvent{
+ {armarx::Clock::Now()},
+ {core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())}});
srv.introspector->success();
@@ -876,49 +866,48 @@ namespace armarx::navigation::server
void
Navigator::stopAllThreads()
{
- stopIfRunning(replanningTask);
- stopIfRunning(monitorTask);
- }
-
- bool
- Navigator::isStackResultValid() const noexcept
- {
-
- // global planner
- if (config.stack.globalPlanner != nullptr)
- {
- if (not globalPlan.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Global trajectory not yet set.";
- return false;
- }
- }
-
- // local planner
- if (config.stack.localPlanner != nullptr)
- {
- if (not localPlan.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Local trajectory not yet set.";
- return false;
- }
- }
-
- // [[likely]]
- return true;
+ stopIfRunning(runningTask);
}
- const core::Trajectory& Navigator::currentTrajectory() const
- {
- ARMARX_CHECK(isStackResultValid());
-
- if(localPlan.has_value())
- {
- return localPlan->trajectory;
- }
-
- return globalPlan->trajectory;
- }
+ // bool
+ // Navigator::isStackResultValid() const noexcept
+ // {
+
+ // // global planner
+ // if (config.stack.globalPlanner != nullptr)
+ // {
+ // if (not globalPlan.has_value())
+ // {
+ // ARMARX_VERBOSE << deactivateSpam(1) << "Global trajectory not yet set.";
+ // return false;
+ // }
+ // }
+
+ // // local planner
+ // if (config.stack.localPlanner != nullptr)
+ // {
+ // if (not localPlan.has_value())
+ // {
+ // ARMARX_VERBOSE << deactivateSpam(1) << "Local trajectory not yet set.";
+ // return false;
+ // }
+ // }
+
+ // // [[likely]]
+ // return true;
+ // }
+
+ // const core::Trajectory& Navigator::currentTrajectory() const
+ // {
+ // ARMARX_CHECK(isStackResultValid());
+
+ // if(localPlan.has_value())
+ // {
+ // return localPlan->trajectory;
+ // }
+
+ // return globalPlan->trajectory;
+ // }
void
Navigator::pause()
@@ -937,6 +926,8 @@ namespace armarx::navigation::server
ARMARX_INFO << "Resume.";
executorEnabled.store(true);
+
+ srv.executor->start();
}
void
diff --git a/source/armarx/navigation/server/Navigator.h b/source/armarx/navigation/server/Navigator.h
index 19318c01..a0affd5d 100644
--- a/source/armarx/navigation/server/Navigator.h
+++ b/source/armarx/navigation/server/Navigator.h
@@ -67,8 +67,7 @@ namespace armarx::navigation::server
{
struct
{
- int replanningUpdatePeriod{10}; // [ms]
- int monitorUpdatePeriod{10}; // [ms]
+ int replanningUpdatePeriod{100}; // [ms]
} tasks;
};
@@ -117,17 +116,18 @@ namespace armarx::navigation::server
void moveToAbsolute(const std::vector<core::Pose>& waypoints);
void moveTowardsAbsolute(const core::Direction& direction);
- void replan();
+ void run();
void updateScene();
- void updateLocalPlanner();
- void updateExecutor();
- void updateIntrospector();
+
+ std::optional<loc_plan::LocalPlannerResult> updateLocalPlanner();
+ void updateExecutor(const std::optional<loc_plan::LocalPlannerResult>& localPlan);
+ void updateIntrospector(const std::optional<loc_plan::LocalPlannerResult>& localPlan);
void updateMonitor();
- const core::Trajectory& currentTrajectory() const;
- bool isStackResultValid() const noexcept;
+ // const core::Trajectory& currentTrajectory() const;
+ // bool isStackResultValid() const noexcept;
void stopAllThreads();
@@ -137,6 +137,10 @@ namespace armarx::navigation::server
void setGraphEdgeCosts(core::Graph& graph) const;
GraphBuilder convertToGraph(const std::vector<client::WaypointTarget>& targets) const;
+ bool hasLocalPlanner() const noexcept
+ {
+ return config.stack.localPlanner != nullptr;
+ }
Config config;
@@ -144,8 +148,7 @@ namespace armarx::navigation::server
std::atomic_bool executorEnabled = true;
- PeriodicTask<Navigator>::pointer_type replanningTask;
- PeriodicTask<Navigator>::pointer_type monitorTask;
+ PeriodicTask<Navigator>::pointer_type runningTask;
std::optional<GoalReachedMonitor> goalReachedMonitor;
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
index 0882171f..a4dfe4c2 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -9,8 +9,8 @@
#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/controller_descriptions.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>
@@ -21,32 +21,47 @@ namespace armarx::navigation::server
ControllerComponentPlugin& controllerComponentPlugin) :
controllerPlugin(controllerComponentPlugin)
{
+ ARMARX_TRACE;
controllerComponentPlugin.getRobotUnitPlugin().getRobotUnit()->loadLibFromPackage(
"armarx_navigation", "libarmarx_navigation_platform_controller.so");
// make default configs available to the memory
ARMARX_INFO << "Loading default configs";
{
- const std::string configBasePath =
- PackagePath("armarx_navigation", "controller_config").toSystemPath();
+ // const std::filesystem::path configBasePath =
+ // PackagePath("armarx_navigation", "controller_config").toSystemPath();
- armarx::control::memory::config::parseAndStoreDefaultConfigs<
- armarx::navigation::common::ControllerType::PlatformTrajectory>(
- configBasePath, controllerComponentPlugin.configMemoryWriter());
+ ARMARX_TRACE;
+ // armarx::control::memory::config::parseAndStoreDefaultConfigs<
+ // armarx::navigation::common::ControllerType::PlatformTrajectory>(
+ // "" /*configBasePath*/, controllerComponentPlugin.configMemoryWriter());
+ ARMARX_TRACE;
ARMARX_INFO << "asdlfasfdlh";
}
// initialize controller
ARMARX_INFO << "Initializing controller";
{
+ ARMARX_TRACE;
auto builder = controllerPlugin.createControllerBuilder<
armarx::navigation::common::ControllerType::PlatformTrajectory>();
- auto ctrlWrapper = builder.create();
+ ARMARX_TRACE;
+ // FIXME remove withconfig
+ auto ctrlWrapper =
+ builder.withNodeSet("PlatformPlanning")
+ .withConfig(
+ "/home/fabi/workspace/armarx/skills/navigation/data/armarx_navigation/"
+ "controller_config/PlatformTrajectory/default.json")
+ .create();
+
+ ARMARX_TRACE;
+ ARMARX_CHECK(ctrlWrapper.has_value());
ctrl.emplace(std::move(ctrlWrapper.value()));
}
+ ARMARX_TRACE;
ARMARX_CHECK(ctrl.has_value());
ARMARX_INFO << "PlatformControllerExecutor: init done.";
}
@@ -57,6 +72,9 @@ namespace armarx::navigation::server
void
PlatformControllerExecutor::execute(const core::Trajectory& trajectory)
{
+ ARMARX_INFO << "Received trajectory for execution with " << trajectory.points().size()
+ << " points";
+
toAron(ctrl->config.targets.trajectory, trajectory);
// sends the updated config to the controller and stores it in the memory
--
GitLab
From 5532a311eed5c15daad931031e8b4bc2b0377a68 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 13 Jul 2022 13:29:35 +0200
Subject: [PATCH 34/47] platform_controller: tested in simulation
---
.../PlatformTrajectoryController.cpp | 79 +++++++++++++++++--
1 file changed, 74 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
index b1fd2da7..91106344 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
@@ -1,21 +1,31 @@
#include "PlatformTrajectoryController.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/navigation/common/controller_types.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
-#include <armarx/navigation/platform_controller/aron_conversions.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_conversions.h>
namespace armarx::navigation::platform_controller::platform_trajectory
{
+ const NJointControllerRegistration<Controller> Registration(
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformTrajectory));
+
Controller::Controller(const RobotUnitPtr& robotUnit,
const NJointControllerConfigPtr& config,
const VirtualRobot::RobotPtr&)
{
-
+ ARMARX_IMPORTANT << "Creating "
+ << common::ControllerTypeNames.to_name(
+ common::ControllerType::PlatformTrajectory);
// config
ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
ARMARX_CHECK_EXPRESSION(cfg);
@@ -32,7 +42,6 @@ namespace armarx::navigation::platform_controller::platform_trajectory
<< "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;
@@ -55,6 +64,11 @@ namespace armarx::navigation::platform_controller::platform_trajectory
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
@@ -68,12 +82,39 @@ namespace armarx::navigation::platform_controller::platform_trajectory
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 armarx::navigation::traj_ctrl::TrajectoryControllerResult 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
@@ -81,16 +122,44 @@ namespace armarx::navigation::platform_controller::platform_trajectory
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);
+
+ debugObservers->setDebugChannel(
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformTrajectory),
+ 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);
}
--
GitLab
From 9041661416955055c94c22f89d412a6c602a9333 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 13 Jul 2022 13:30:04 +0200
Subject: [PATCH 35/47] minor
---
source/armarx/navigation/components/Navigator/Navigator.cpp | 4 +++-
1 file changed, 3 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 26ae33d2..adc02e00 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -100,7 +100,6 @@ namespace armarx::navigation
std::vector<core::Pose>
convert(const std::vector<Eigen::Matrix4f>& wps)
{
- using namespace armarx::navigation;
std::vector<core::Pose> p;
p.reserve(wps.size());
std::transform(wps.begin(),
@@ -125,6 +124,7 @@ namespace armarx::navigation::components
addPlugin(eventsWriterPlugin);
addPlugin(resultsWriterPlugin);
addPlugin(graphReaderPlugin);
+ addPlugin(costmapReaderPlugin);
addPlugin(costmapWriterPlugin);
addPlugin(virtualRobotReaderPlugin);
@@ -181,8 +181,10 @@ namespace armarx::navigation::components
initializeScene();
+ ARMARX_TRACE;
executor.emplace(server::PlatformControllerExecutor(getControlComponentPlugin()));
+ ARMARX_TRACE;
introspector = server::ArvizIntrospector(getArvizClient(), scene().robot);
--
GitLab
From e3ebd7570bd9fdfdb6c7f5e9148f769ebe68a974 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 14 Jul 2022 07:47:53 +0200
Subject: [PATCH 36/47] scenario update
---
...ulatedObjectLocalizerDynamicSimulation.cfg | 8 ----
.../HandUnitDynamicSimulationApp.LeftHand.cfg | 17 +++++++
...HandUnitDynamicSimulationApp.RightHand.cfg | 17 +++++++
.../config/ObjectMemory.cfg | 48 +++++++++++++++----
.../config/RobotStateMemory.cfg | 16 -------
.../config/RobotUnitSimulationApp.cfg | 2 +-
.../SelfLocalizationDynamicSimulationApp.cfg | 18 +------
.../config/VisionMemory.cfg | 16 -------
.../PlatformNavigation/PlatformNavigation.scx | 2 +-
9 files changed, 77 insertions(+), 67 deletions(-)
diff --git a/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg b/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
index a64fd58d..7c4574e0 100644
--- a/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
+++ b/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
@@ -133,14 +133,6 @@ ArmarX.ArticulatedObjectLocalizerDynamicSimulation.mem.obj.articulated.ProviderN
# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.objects = Default value not mapped.
-# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.tpc.sub.MemoryListener = MemoryUpdates
-
-
# 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
diff --git a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
index 18e2d2db..aa21b8fd 100644
--- a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
+++ b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
@@ -139,6 +139,23 @@ ArmarX.HandUnitDynamicSimulation.ObjectName = LeftHandUnit
# ArmarX.HandUnitDynamicSimulation.SimulatorProxyName = Simulator
+# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory: Require the legacy MemoryX working memory to be available before starting.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory = false
+
+
+# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName: Name of the object pose storage (only used if necessary).
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName = ObjectMemory
+
+
# ArmarX.HandUnitDynamicSimulation.inheritFrom: No Description
# Attributes:
# - Default: RobotConfig
diff --git a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
index 85cc4345..b92bb000 100644
--- a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
+++ b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
@@ -139,6 +139,23 @@ ArmarX.HandUnitDynamicSimulation.ObjectName = RightHandUnit
# ArmarX.HandUnitDynamicSimulation.SimulatorProxyName = Simulator
+# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory: Require the legacy MemoryX working memory to be available before starting.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory = false
+
+
+# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName: Name of the object pose storage (only used if necessary).
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName = ObjectMemory
+
+
# ArmarX.HandUnitDynamicSimulation.inheritFrom: No Description
# Attributes:
# - Default: RobotConfig
diff --git a/scenarios/NavigationSimulation/config/ObjectMemory.cfg b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
index 58cd471b..57b383ba 100644
--- a/scenarios/NavigationSimulation/config/ObjectMemory.cfg
+++ b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
@@ -142,14 +142,6 @@ ArmarX.ObjectMemory.MinimumLoggingLevel = Debug
# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
-# ArmarX.ObjectMemory.RemoteStateComponentName: Name of the robot state component
-# Attributes:
-# - Default: RobotStateComponent
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.RemoteStateComponentName = RobotStateComponent
-
-
# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
# Attributes:
# - Default: KinematicUnitObserver
@@ -641,6 +633,38 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.mem.ltm.storagepath = Default value not mapped.
+# 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.descriptionSegment:
+# Attributes:
+# - Default: Description
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.descriptionSegment = Description
+
+
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment:
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment:
+# Attributes:
+# - Default: Proprioception
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment = Proprioception
+
+
# 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:
@@ -667,6 +691,14 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.prediction.TimeWindow = 2
+# ArmarX.ObjectMemory.robotName:
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.robotName = Armar6
+
+
# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
# - Default: DebugObserver
diff --git a/scenarios/NavigationSimulation/config/RobotStateMemory.cfg b/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
index 5806a1ce..71c1ac65 100644
--- a/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
+++ b/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
@@ -435,22 +435,6 @@ ArmarX.RobotStateMemory.seg.localization.MaxHistorySize = 3000
ArmarX.RobotStateMemory.seg.proprioception.MaxHistorySize = 3000
-# ArmarX.RobotStateMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotStateMemory.tpc.pub.MemoryListener = MemoryUpdates
-
-
-# ArmarX.RobotStateMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotStateMemory.tpc.sub.MemoryListener = MemoryUpdates
-
-
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
# - Default: 0
diff --git a/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg b/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
index a02e6d4b..47323755 100644
--- a/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
+++ b/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
@@ -363,7 +363,7 @@ ArmarX.RobotUnitSimulation.InverseDynamicsRobotJointSets = PlatformRightArm
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-ArmarX.RobotUnitSimulation.MinimumLoggingLevel = Debug
+ArmarX.RobotUnitSimulation.MinimumLoggingLevel = Info
# ArmarX.RobotUnitSimulation.NjointController_AllowedExecutionTimePerControlDeviceUntilError: An Error will be printed, If the execution time in micro seconds of a NJointControllerBase exceeds this parameter times the number of ControlDevices.
diff --git a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
index 1dab8f00..23b096b3 100644
--- a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
+++ b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
@@ -175,14 +175,6 @@ ArmarX.SelfLocalizationDynamicSimulation.RobotName = Armar6
# ArmarX.SelfLocalizationDynamicSimulation.cmp.Simulator = Simulator
-# ArmarX.SelfLocalizationDynamicSimulation.cmp.WorkingMemory: Ice object name of the `WorkingMemory` component.
-# Attributes:
-# - Default: WorkingMemory
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SelfLocalizationDynamicSimulation.cmp.WorkingMemory = WorkingMemory
-
-
# ArmarX.SelfLocalizationDynamicSimulation.cycleTime:
# Attributes:
# - Default: 30
@@ -191,7 +183,7 @@ ArmarX.SelfLocalizationDynamicSimulation.RobotName = Armar6
# ArmarX.SelfLocalizationDynamicSimulation.cycleTime = 30
-# ArmarX.SelfLocalizationDynamicSimulation.longterm_memory: Ice object name of the `LongtermMemory` component.
+# ArmarX.SelfLocalizationDynamicSimulation.longterm_memory: Which legacy long-term memory to use if longterm_memory.updateor longterm_memory.retrieve_initial_pose are set
# Attributes:
# - Default: LongtermMemory
# - Case sensitivity: yes
@@ -307,14 +299,6 @@ ArmarX.SelfLocalizationDynamicSimulation.mem.robot_state.Memory = RobotState
# ArmarX.SelfLocalizationDynamicSimulation.tpc.pub.PlatformUnit = PlatformUnit
-# ArmarX.SelfLocalizationDynamicSimulation.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SelfLocalizationDynamicSimulation.tpc.sub.MemoryListener = MemoryUpdates
-
-
# ArmarX.SelfLocalizationDynamicSimulation.working_memory.update: If enabled, updates the global pose in the working memory
# Attributes:
# - Default: true
diff --git a/scenarios/NavigationSimulation/config/VisionMemory.cfg b/scenarios/NavigationSimulation/config/VisionMemory.cfg
index 9c59a9f6..22e0d3e6 100644
--- a/scenarios/NavigationSimulation/config/VisionMemory.cfg
+++ b/scenarios/NavigationSimulation/config/VisionMemory.cfg
@@ -364,19 +364,3 @@
# ArmarX.VisionMemory.pointCloudMaxHistorySize = 20
-# ArmarX.VisionMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.tpc.pub.MemoryListener = MemoryUpdates
-
-
-# ArmarX.VisionMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.tpc.sub.MemoryListener = MemoryUpdates
-
-
diff --git a/scenarios/PlatformNavigation/PlatformNavigation.scx b/scenarios/PlatformNavigation/PlatformNavigation.scx
index 1f20100c..a50c5adc 100644
--- a/scenarios/PlatformNavigation/PlatformNavigation.scx
+++ b/scenarios/PlatformNavigation/PlatformNavigation.scx
@@ -6,7 +6,7 @@
<application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="navigator" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="navigation_memory" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="example_client" instance="" package="armarx_navigation" nodeName="" enabled="false" 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"/>
</scenario>
--
GitLab
From ccc832f82db349c7010fbbdb8aff07b9d09f3a77 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 14 Jul 2022 08:05:23 +0200
Subject: [PATCH 37/47] fixing includes and cmake targets
---
.../PlatformTrajectoryController.h | 11 ++++-------
source/armarx/navigation/server/CMakeLists.txt | 2 +-
2 files changed, 5 insertions(+), 8 deletions(-)
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
index 9dac5f6b..a54d44b2 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
@@ -29,12 +29,9 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
-#include <armarx/control/common/control_law/ControllerCommonInterface.h>
-#include <armarx/control/njoint_controller/task_space/ControllerInterface.h>
-#include <armarx/control/njoint_qp_controller/ControllerInterface.h>
-
+#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
namespace armarx
{
@@ -88,7 +85,7 @@ namespace armarx::navigation::platform_controller::platform_trajectory
virtual public armarx::control::ConfigurableNJointControllerInterface
{
public:
- using ConfigPtrT = NJointTaskspaceControllerConfigPtr;
+ using ConfigPtrT = control::ConfigurableNJointControllerInterfacePtr;
Controller(const RobotUnitPtr& robotUnit,
const NJointControllerConfigPtr& config,
@@ -149,4 +146,4 @@ namespace armarx::navigation::platform_controller::platform_trajectory
// const NameValueMap& targetJointAngles) const;
};
-} // namespace armarx::control::platform_controller::platform_trajectory
+} // namespace armarx::navigation::platform_controller::platform_trajectory
diff --git a/source/armarx/navigation/server/CMakeLists.txt b/source/armarx/navigation/server/CMakeLists.txt
index 0e08f94b..a7dad944 100644
--- a/source/armarx/navigation/server/CMakeLists.txt
+++ b/source/armarx/navigation/server/CMakeLists.txt
@@ -57,7 +57,7 @@ armarx_add_library(server
armarx_navigation::safety_control
armarx_navigation::memory
armarx_control::client
- armarx_control::njoint_qp_controller_ice
+ armarx_control::interface
DEPENDENCIES_PRIVATE
range-v3::range-v3
)
--
GitLab
From 2a224d423ff771f393565e7bf94c64ad5a99d36b Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Mon, 25 Jul 2022 12:59:02 +0200
Subject: [PATCH 38/47] cmake fixes
---
source/armarx/navigation/platform_controller/CMakeLists.txt | 2 +-
.../platform_controller/PlatformTrajectoryController.cpp | 2 +-
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/platform_controller/CMakeLists.txt b/source/armarx/navigation/platform_controller/CMakeLists.txt
index abeeec07..509c8f9a 100644
--- a/source/armarx/navigation/platform_controller/CMakeLists.txt
+++ b/source/armarx/navigation/platform_controller/CMakeLists.txt
@@ -17,7 +17,7 @@ armarx_add_library(platform_controller
DEPENDENCIES_PUBLIC
Simox::VirtualRobot
armarx_control::common
- armarx_control::njoint_qp_controller_ice
+ 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/PlatformTrajectoryController.cpp
index 91106344..a95f6d38 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
@@ -9,7 +9,7 @@
#include <armarx/control/common/aron_conversions.h>
#include <armarx/control/common/type.h>
-#include <armarx/control/common/utils.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_conversions.h>
--
GitLab
From 92d419c5007d9def27d659e2a74f7ab601280f10 Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Mon, 25 Jul 2022 12:59:16 +0200
Subject: [PATCH 39/47] fix: absolute path
---
.../server/execution/PlatformControllerExecutor.cpp | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
index a4dfe4c2..5f089038 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -48,12 +48,12 @@ namespace armarx::navigation::server
armarx::navigation::common::ControllerType::PlatformTrajectory>();
ARMARX_TRACE;
- // FIXME remove withconfig
+
+ const armarx::PackagePath configPath("armarx_navigation", "controller_config/PlatformTrajectory/default.json");
+
auto ctrlWrapper =
builder.withNodeSet("PlatformPlanning")
- .withConfig(
- "/home/fabi/workspace/armarx/skills/navigation/data/armarx_navigation/"
- "controller_config/PlatformTrajectory/default.json")
+ .withConfig(configPath.toSystemPath())
.create();
ARMARX_TRACE;
--
GitLab
From 29fdf599d755f63a1a1ea492eb1e6f1276f298cb Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Mon, 25 Jul 2022 13:02:58 +0200
Subject: [PATCH 40/47] fix: config ptr type
---
.../platform_controller/PlatformTrajectoryController.h | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
index a54d44b2..2b47dc98 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
@@ -85,7 +85,7 @@ namespace armarx::navigation::platform_controller::platform_trajectory
virtual public armarx::control::ConfigurableNJointControllerInterface
{
public:
- using ConfigPtrT = control::ConfigurableNJointControllerInterfacePtr;
+ using ConfigPtrT = control::ConfigurableNJointControllerConfigPtr;
Controller(const RobotUnitPtr& robotUnit,
const NJointControllerConfigPtr& config,
--
GitLab
From 271222343c1310b183a04b4325e4b2fae90634ad Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Mon, 25 Jul 2022 14:41:41 +0200
Subject: [PATCH 41/47] minor
---
source/armarx/navigation/components/Navigator/Navigator.cpp | 1 +
source/armarx/navigation/components/Navigator/Navigator.h | 1 +
2 files changed, 2 insertions(+)
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index adc02e00..ddb4f811 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -186,6 +186,7 @@ namespace armarx::navigation::components
ARMARX_TRACE;
introspector = server::ArvizIntrospector(getArvizClient(), scene().robot);
+ // memoryIntrospector = server::MemoryIntrospector(resultsWriterPlugin->get(), );
navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index d1f37ccc..7c725c31 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -168,6 +168,7 @@ namespace armarx::navigation::components
std::optional<server::PlatformControllerExecutor> executor;
std::optional<server::ArvizIntrospector> introspector;
+ // std::optional<server::MemoryIntrospector> memoryIntrospector;
std::optional<server::scene_provider::SceneProvider> sceneProvider;
std::unordered_map<std::string, server::Navigator> navigators;
--
GitLab
From 84c9c92f2fadd0281627d8830510b53c8194f8c2 Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Mon, 25 Jul 2022 14:41:54 +0200
Subject: [PATCH 42/47] fix: synchronizing scene initially
---
source/armarx/navigation/server/Navigator.cpp | 2 ++
1 file changed, 2 insertions(+)
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 2be9420b..26479047 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -655,6 +655,8 @@ namespace armarx::navigation::server
ARMARX_TRACE;
ARMARX_CHECK_NOT_EMPTY(waypoints);
+ srv.sceneProvider->synchronize(Clock::Now());
+
ARMARX_INFO << "Request to move from " << srv.sceneProvider->scene().robot->getGlobalPose()
<< " to " << waypoints.back().matrix();
--
GitLab
From d2f4431d2d6070fbde130626d2dcb3aff1fbf6e9 Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Mon, 25 Jul 2022 14:42:14 +0200
Subject: [PATCH 43/47] debug stuff
---
source/armarx/navigation/server/Navigator.cpp | 14 ++++++++++----
.../TrajectoryFollowingController.cpp | 1 +
2 files changed, 11 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 26479047..e43dfa69 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -728,15 +728,19 @@ namespace armarx::navigation::server
// scene update
{
+ ARMARX_DEBUG << "Updating scene";
+
const Duration duration =
armarx::core::time::StopWatch::measure([&]() { updateScene(); });
- ARMARX_VERBOSE << deactivateSpam(0.2) << "Scene update: " << duration.toSecondsDouble()
- << "s.";
+ ARMARX_VERBOSE << deactivateSpam(0.2) << "Scene update: " << duration.toMilliSecondsDouble()
+ << "ms.";
}
// local planner update
{
+ ARMARX_DEBUG << "Local planner update";
+
const Duration duration = armarx::core::time::StopWatch::measure(
[&]()
{
@@ -748,16 +752,18 @@ namespace armarx::navigation::server
}
});
ARMARX_VERBOSE << deactivateSpam(0.2)
- << "Local planner update: " << duration.toSecondsDouble() << "s.";
+ << "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
}
// monitor update
{
+ ARMARX_DEBUG << "Monitor update";
+
const Duration duration =
armarx::core::time::StopWatch::measure([&]() { updateMonitor(); });
ARMARX_VERBOSE << deactivateSpam(0.2)
- << "Monitor update: " << duration.toSecondsDouble() << "s.";
+ << "Monitor update: " << duration.toMilliSecondsDouble() << "ms.";
}
}
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index b5ef2b45..17431d8e 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -11,6 +11,7 @@
#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/TrajectoryController.h>
--
GitLab
From 7f6fc1b4004dbd11ed9e740bf82ec93497523bab Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Mon, 25 Jul 2022 14:42:28 +0200
Subject: [PATCH 44/47] TrajectoryFollowingController: if trajectory is empty:
don't move
---
.../trajectory_control/TrajectoryFollowingController.cpp | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index 17431d8e..2ae7f289 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -131,6 +131,14 @@ namespace armarx::navigation::traj_ctrl
{
ARMARX_CHECK_NOT_NULL(robot) << "Robot not available";
+ if (trajectory.points().empty())
+ {
+ ARMARX_INFO << "Trajectory is empty.";
+ return TrajectoryControllerResult{
+ .twist = core::Twist::Zero(),
+ .dropPoint = {.waypoint = {.pose = core::Pose::Identity()}, .velocity = 0}};
+ }
+
const core::Pose currentPose(robot->getGlobalPose());
const auto projectedPose = trajectory.getProjection(
--
GitLab
From ee2f1ee9bd254a7572b8f52e5f30890e8dcc80b1 Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Mon, 25 Jul 2022 14:45:19 +0200
Subject: [PATCH 45/47] rt controller: logging
---
.../platform_controller/PlatformTrajectoryController.cpp | 1 +
1 file changed, 1 insertion(+)
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
index a95f6d38..b5da2a90 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
@@ -127,6 +127,7 @@ 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());
debugObservers->setDebugChannel(
common::ControllerTypeNames.to_name(common::ControllerType::PlatformTrajectory),
--
GitLab
From 44f3f88f76e4b51863388d5cef38f6de34aa57d4 Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit.edu>
Date: Mon, 25 Jul 2022 20:01:00 +0200
Subject: [PATCH 46/47] fix: TrajectoryFollowingController: returing twist in
local frame
---
.../TrajectoryFollowingController.cpp | 12 +++++++++++-
1 file changed, 11 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index 2ae7f289..b7fcdb2d 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -203,7 +203,17 @@ namespace armarx::navigation::traj_ctrl
ARMARX_VERBOSE << deactivateSpam(1) << "Twist limited " << twistLimited.linear.transpose();
ARMARX_VERBOSE << deactivateSpam(1) << "Twist angular " << twistLimited.angular.transpose();
- return TrajectoryControllerResult{.twist = twistLimited,
+ // 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,
.dropPoint = projectedPose.projection};
}
--
GitLab
From 1b34e305888c9237152a6decf82b209ea158ea22 Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit.edu>
Date: Mon, 25 Jul 2022 20:04:36 +0200
Subject: [PATCH 47/47] cleanup
---
.../components/Navigator/Navigator.cpp | 152 ------------------
1 file changed, 152 deletions(-)
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index ddb4f811..3829c503 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -218,23 +218,6 @@ namespace armarx::navigation::components
{
}
- // VirtualRobot::RobotPtr
- // components::Navigator::getRobot()
- // {
- // ARMARX_TRACE;
- // auto robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(
- // getControlComponentPlugin()
- // .getRobotUnitPlugin()
- // .getRobotUnit()
- // ->getKinematicUnit()
- // ->getRobotName(),
- // VirtualRobot::RobotIO::RobotDescription::eCollisionModel,
- // true);
-
- // ARMARX_CHECK_NOT_NULL(robot);
- // return robot;
- // }
-
void
components::Navigator::initializeScene()
{
@@ -494,141 +477,6 @@ namespace armarx::navigation::components
arviz.commit(layer);
}
- // core::StaticScene
- // components::Navigator::staticScene()
- // {
- // ARMARX_TRACE;
-
- // 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();
-
- // ARMARX_INFO << "Storing costmap in memory";
- // costmapWriterPlugin->get().store(
- // costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
-
- // ARMARX_INFO << "Done";
-
- // ARMARX_TRACE;
- // const auto grid = costmap.getGrid();
-
- // core::StaticScene scene{.objects = objects,
- // .costmap = std::make_unique<algorithms::Costmap>(costmap)};
-
- // ARMARX_INFO << "The object scene consists of " << scene().objects->getSize() << " objects";
-
-
- // // visualize(costmap, arviz, "distance");
-
- // /*
- // const armem::vision::occupancy_grid::client::Reader::Result result =
- // occupancyGridReader.query(armem::vision::occupancy_grid::client::Reader::Query{
- // .providerName = "CartographerMappingAndLocalization",
- // .timestamp = armem::Time::Now()});
-
- // if (result and result.occupancyGrid.has_value() and false) // Feature disabled on master ...
- // {
- // ARMARX_INFO << "Occupancy grid available!";
-
- // const auto occupancyGridSceneElements = util::asSceneObjects(
- // result.occupancyGrid.value(),
- // OccupancyGridHelper::Params{.freespaceThreshold = 0.45F,
- // .occupiedThreshold = params.occupiedGridThreshold});
- // ARMARX_INFO << occupancyGridSceneElements->getSize()
- // << " scene elements from occupancy grid";
-
- // scene().objects->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});
- // }
- // else
- // {
- // ARMARX_INFO << "Occupancy grid not available";
- // }*/
-
- // return scene;
- // }
-
-
- // void
- // components::Navigator::updateRobot()
- // {
- // const auto timestamp = armem::Time::Now();
-
- // auto& virtualRobotReader = virtualRobotReaderPlugin->get();
-
- // ARMARX_TRACE;
- // ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*scene().robot, timestamp));
-
- // ARMARX_TRACE;
- // const auto robotDescription =
- // virtualRobotReader.queryDescription(scene().robot->getType(), 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!
- // {
- // if (not scene().platformVelocity.has_value())
- // {
- // scene().platformVelocity = core::Twist();
- // }
-
- // scene().platformVelocity->linear = platformState->twist.linear;
- // scene().platformVelocity->angular = platformState->twist.angular;
- // }
- // }
-
server::Navigator*
components::Navigator::activeNavigator()
{
--
GitLab