From 916120b7b0b1c28c6984b429937746fe10024f41 Mon Sep 17 00:00:00 2001
From: "Christian R. G. Dreher" <c.dreher@kit.edu>
Date: Fri, 9 Jul 2021 14:24:59 +0200
Subject: [PATCH] feature: Implement pausing and resuming navigation.
---
.../Navigation/libraries/server/Navigator.cpp | 27 +++++++++--
.../Navigation/libraries/server/Navigator.h | 46 +++++++++++++------
2 files changed, 54 insertions(+), 19 deletions(-)
diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp
index caa798e8..3ec4c60a 100644
--- a/source/Navigation/libraries/server/Navigator.cpp
+++ b/source/Navigation/libraries/server/Navigator.cpp
@@ -1,13 +1,16 @@
#include "Navigator.h"
+
#include <algorithm>
+
#include <Eigen/src/Geometry/Transform.h>
#include <VirtualRobot/Robot.h>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <Navigation/libraries/core/types.h>
-#include "Navigation/libraries/core/types.h"
namespace armarx::nav::server
{
@@ -47,7 +50,7 @@ namespace armarx::nav::server
Navigator::~Navigator()
{
- stop();
+ pauseMovement();
}
void Navigator::moveTo(const std::vector<core::Pose>& waypoints, const core::NavigationFrames& navigationFrame)
@@ -99,23 +102,37 @@ namespace armarx::nav::server
res.safeVelocity = stack.safetyControl->control(res.controlVelocity.value());
}
- executor->move(res.velocity());
+ if (movementEnabled)
+ {
+ executor->move(res.velocity());
+ }
}
}
+
void Navigator::moveTowards(const core::Direction& direction, const core::NavigationFrames& navigationFrame)
{
}
+
void Navigator::moveTowardsAbsolute(const core::Direction& direction)
{
}
- void Navigator::stop()
+
+ void Navigator::pauseMovement()
{
+ movementEnabled = false;
+ const core::Twist zero{Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()};
+ executor->move(zero);
+ }
+
+ void Navigator::resumeMovement()
+ {
+ movementEnabled = true;
}
} // namespace armarx::nav::server
diff --git a/source/Navigation/libraries/server/Navigator.h b/source/Navigation/libraries/server/Navigator.h
index 091aa639..70728823 100644
--- a/source/Navigation/libraries/server/Navigator.h
+++ b/source/Navigation/libraries/server/Navigator.h
@@ -19,10 +19,15 @@
* GNU General Public License
*/
+
#pragma once
+
+// STD/STL
+#include <atomic>
#include <optional>
+// Eigen
#include <Eigen/Core>
// ArmarX
@@ -34,18 +39,11 @@
#include "NavigationStack.h"
#include <Navigation/libraries/server/execution/ExecutorInterface.h>
+
namespace armarx::nav::server
{
- struct StackResult
- {
- core::TrajectoryPtr globalTrajectory;
- core::TrajectoryPtr localTrajectory;
- std::optional<core::Twist> controlVelocity;
- std::optional<core::Twist> safeVelocity;
- core::TrajectoryPtr trajectory() const;
- core::Twist velocity() const;
- };
+ struct StackResult;
class Navigator : public armarx::Logging
{
@@ -53,20 +51,40 @@ namespace armarx::nav::server
public:
Navigator(const server::NavigationStack& navigationStack, const core::Scene& scene, ExecutorInterface& executor);
- virtual ~Navigator();
void moveTo(const std::vector<core::Pose>& waypoints, const core::NavigationFrames& navigationFrame);
- void moveToAbsolute(const std::vector<core::Pose>& waypoints);
-
void moveTowards(const core::Direction& direction, const core::NavigationFrames& navigationFrame);
- void moveTowardsAbsolute(const core::Direction& direction);
- void stop();
+ void pauseMovement();
+ void resumeMovement();
+
+ // Non-API
+ Navigator(Navigator&& other) : stack{std::move(other.stack)}, scene{other.scene}, executor{other.executor}, movementEnabled{other.movementEnabled.load()} {}
+ virtual ~Navigator();
+
+ private:
+
+ void moveToAbsolute(const std::vector<core::Pose>& waypoints);
+ void moveTowardsAbsolute(const core::Direction& direction);
private:
+
server::NavigationStack stack;
const core::Scene* scene;
ExecutorInterface* executor;
+ std::atomic_bool movementEnabled = true;
};
+
+ struct StackResult // TODO: Refactor into own file
+ {
+ core::TrajectoryPtr globalTrajectory;
+ core::TrajectoryPtr localTrajectory;
+ std::optional<core::Twist> controlVelocity;
+ std::optional<core::Twist> safeVelocity;
+
+ core::TrajectoryPtr trajectory() const;
+ core::Twist velocity() const;
+ };
+
} // namespace armarx::nav::server
--
GitLab