diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp index caa798e80b4c8b0aae25ea38f548ddc9cd7a6bb3..0290679b6d62c4a0995be015161c642847ed1a86 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.load()) + { + 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.store(false); + const core::Twist zero{Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()}; + executor->move(zero); + } + + void Navigator::resumeMovement() + { + movementEnabled.store(true); } } // namespace armarx::nav::server diff --git a/source/Navigation/libraries/server/Navigator.h b/source/Navigation/libraries/server/Navigator.h index 091aa639f4bbb0e0bcdc3f90f135f66e28d5e114..70728823c6f0e72e5ded3fc92556ae416ef12a2d 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