From 65d88f93385ce875aa541e77337db40594ce19fa Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Sun, 11 Jul 2021 21:27:40 +0200
Subject: [PATCH] update
---
.../components/Navigator/Navigator.cpp | 110 ++++++++++++++----
.../components/Navigator/Navigator.h | 8 +-
.../components/Navigator/RemoteGui.cpp | 34 ++++--
.../libraries/algorithms/CMakeLists.txt | 6 +
.../algorithms/astar/AStarPlanner.cpp | 2 +
.../smoothing/ChainApproximation.cpp | 4 +-
.../algorithms/smoothing/ChainApproximation.h | 4 +-
.../smoothing/CircularPathSmoothing.cpp | 82 +++++++++++++
.../smoothing/CircularPathSmoothing.h | 69 +----------
.../client/NavigationStackConfig.cpp | 24 ++--
.../libraries/client/NavigationStackConfig.h | 9 +-
.../Navigation/libraries/core/Trajectory.cpp | 73 +++++++++++-
source/Navigation/libraries/core/Trajectory.h | 18 ++-
source/Navigation/libraries/core/constants.h | 26 +++--
source/Navigation/libraries/core/fwd.h | 33 ++++++
source/Navigation/libraries/core/types.h | 4 +
.../factories/GlobalPlannerFactory.h | 5 +-
.../libraries/factories/LocalPlannerFactory.h | 6 +-
.../factories/NavigationStackFactory.cpp | 12 +-
.../factories/NavigationStackFactory.h | 4 +
.../factories/SafetyControllerFactory.h | 5 +-
.../factories/TrajectoryControllerFactory.h | 6 +-
source/Navigation/libraries/factories/fwd.h | 53 +++++++++
.../libraries/global_planning/AStar.cpp | 67 ++++++++++-
.../libraries/global_planning/AStar.h | 5 +
.../libraries/global_planning/GlobalPlanner.h | 2 +-
.../Navigation/libraries/server/Navigator.cpp | 79 ++++++++++---
.../Navigation/libraries/server/Navigator.h | 2 +
.../server/monitoring/GoalReachedMonitor.cpp | 7 +-
29 files changed, 596 insertions(+), 163 deletions(-)
create mode 100644 source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.cpp
create mode 100644 source/Navigation/libraries/core/fwd.h
create mode 100644 source/Navigation/libraries/factories/fwd.h
diff --git a/source/Navigation/components/Navigator/Navigator.cpp b/source/Navigation/components/Navigator/Navigator.cpp
index 4e0714af..f4ef750e 100644
--- a/source/Navigation/components/Navigator/Navigator.cpp
+++ b/source/Navigation/components/Navigator/Navigator.cpp
@@ -23,6 +23,7 @@
#include "Navigator.h"
+#include <algorithm>
#include <cmath>
#include <iterator>
#include <memory>
@@ -38,22 +39,20 @@
#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include "ArmarXCore/core/services/tasks/TaskUtil.h"
#include "ArmarXCore/util/CPPUtility/trace.h"
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
-// Navigation
-#include "AStar.h"
+#include "Navigation/components/Navigator/RemoteGui.h"
#include "Navigation/libraries/client/NavigationStackConfig.h"
#include "Navigation/libraries/core/types.h"
-// #include "Navigation/libraries/factories/NavigationStackFactory.h"
-// #include "Navigation/libraries/global_planning/Point2Point.h"
#include "Navigation/libraries/server/Navigator.h"
#include "Navigation/libraries/server/execution/PlatformUnitExecutor.h"
-// #include "Navigation/libraries/trajectory_control/TrajectoryFollowingController.h"
+#include "Navigation/libraries/factories/NavigationStackFactory.h"
#include "Navigation/libraries/util/util.h"
-#include "RemoteGui.h"
armarx::nav::components::Navigator::Navigator() = default;
armarx::nav::components::Navigator::~Navigator() = default;
@@ -64,6 +63,15 @@ void armarx::nav::components::Navigator::onInitComponent()
void armarx::nav::components::Navigator::onConnectComponent()
{
+ static bool initialized{false};
+
+ // redirect to onReconnect to avoid any setup issues
+ if (initialized)
+ {
+ ARMARX_INFO << "Reconnecting ...";
+ onReconnectComponent();
+ return;
+ }
// TODO check if reconnecting
@@ -77,16 +85,33 @@ void armarx::nav::components::Navigator::onConnectComponent()
// TODO memory
// TODO param (10)
- robotStateUpdateTask = new PeriodicTask<Navigator>(this, &Navigator::updateRobot, 10);
+ robotStateUpdateTask = new PeriodicTask<Navigator>(
+ this, &Navigator::updateRobot, 10, false, "RobotStateUpdateTask");
robotStateUpdateTask->start();
navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
navRemoteGui->enable();
+
+ initialized = true;
+}
+
+void armarx::nav::components::Navigator::onReconnectComponent()
+{
+ robotStateUpdateTask->start();
+
+ // TODO not in all cases meaningful
+ resumeMovement();
+
+ navRemoteGui->enable();
}
void armarx::nav::components::Navigator::onDisconnectComponent()
{
robotStateUpdateTask->stop();
+
+ pauseMovement();
+
+ navRemoteGui->disable();
}
void armarx::nav::components::Navigator::onExitComponent()
@@ -142,12 +167,22 @@ void armarx::nav::components::Navigator::moveTo(const std::vector<Eigen::Matrix4
const std::string& callerId,
const Ice::Current&)
{
- // TODO: Error handling.
- ARMARX_INFO << "MoveTo";
+ // ARMARX_INFO << LogSender::eGreen << "MoveTo";
- navigators.at(callerId).moveTo(convert(waypoints),
- core::NavigationFramesMap.from_name(navigationMode));
+ try
+ {
+ navigators.at(callerId).moveTo(convert(waypoints),
+ core::NavigationFramesMap.from_name(navigationMode));
+ }
+ catch (...)
+ {
+ // TODO: Error handling.
+ ARMARX_WARNING << "Failed to execute moveTo()."
+ << "Movement will be paused.";
+ pauseMovement(); // TODO pause movement must not throw
+ throw;
+ }
}
void armarx::nav::components::Navigator::moveTowards(const Eigen::Vector3f& direction,
@@ -165,16 +200,28 @@ void armarx::nav::components::Navigator::moveTowards(const Eigen::Vector3f& dire
void armarx::nav::components::Navigator::pauseMovement(const Ice::Current&)
{
- // TODO
- // find active navigator
- // pause navigator
+ auto* navigator = activeNavigator();
+
+ if (navigator == nullptr)
+ {
+ ARMARX_INFO << "No active navigator. Cannot pause movement!";
+ return;
+ }
+
+ navigator->pauseMovement();
}
void armarx::nav::components::Navigator::resumeMovement(const Ice::Current&)
{
- // TODO
- // find active navigator
- // resume navigator
+ auto* navigator = activeNavigator();
+
+ if (navigator == nullptr)
+ {
+ ARMARX_INFO << "No active navigator. Cannot resume movement!";
+ return;
+ }
+
+ navigator->resumeMovement();
}
armarx::PropertyDefinitionsPtr armarx::nav::components::Navigator::createPropertyDefinitions()
@@ -206,11 +253,11 @@ armarx::nav::core::StaticScene armarx::nav::components::Navigator::staticScene()
{
ARMARX_TRACE;
- core::StaticScene scene;
-
- objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
-
- scene.objects = util::asSceneObjects(objectPoses);
+ const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
+ core::StaticScene scene
+ {
+ .objects = util::asSceneObjects(objectPoses)
+ };
ARMARX_INFO << "The scene consists of " << scene.objects->getSize() << " objects";
@@ -238,3 +285,22 @@ void armarx::nav::components::Navigator::updateRobot()
{
synchronizeLocalClone(scene.robot);
}
+
+armarx::nav::server::Navigator* armarx::nav::components::Navigator::activeNavigator()
+{
+ const auto isActive = [](auto & nameNavPair) -> bool
+ {
+ server::Navigator& navigator = nameNavPair.second;
+ return navigator.isActive();
+ };
+
+ auto it = std::find_if(navigators.begin(), navigators.end(), isActive);
+
+ // no navigator active?
+ if (it == navigators.end())
+ {
+ return nullptr;
+ }
+
+ return &it->second;
+}
diff --git a/source/Navigation/components/Navigator/Navigator.h b/source/Navigation/components/Navigator/Navigator.h
index a6ba65f8..dc42e68e 100644
--- a/source/Navigation/components/Navigator/Navigator.h
+++ b/source/Navigation/components/Navigator/Navigator.h
@@ -46,7 +46,6 @@
#include <Navigation/components/Navigator/NavigatorInterface.h>
#include <Navigation/components/Navigator/RemoteGui.h>
-#include <Navigation/libraries/core/StaticScene.h>
#include <Navigation/libraries/core/types.h>
#include <Navigation/libraries/server/Navigator.h>
#include <Navigation/libraries/server/execution/PlatformUnitExecutor.h>
@@ -114,6 +113,8 @@ namespace armarx::nav::components
/// @see armarx::ManagedIceObject::onConnectComponent()
void onConnectComponent() override;
+ void onReconnectComponent();
+
/// @see armarx::ManagedIceObject::onDisconnectComponent()
void onDisconnectComponent() override;
@@ -127,6 +128,8 @@ namespace armarx::nav::components
VirtualRobot::RobotPtr getRobot();
void updateRobot();
+ server::Navigator* activeNavigator();
+
private:
// TODO update context periodically
@@ -137,7 +140,7 @@ namespace armarx::nav::components
core::Scene scene;
std::optional<server::PlatformUnitExecutor> executor;
std::optional<server::ArvizIntrospector> introspector;
- std::map<std::string, server::Navigator> navigators;
+ std::unordered_map<std::string, server::Navigator> navigators;
std::mutex propertiesMutex;
@@ -145,6 +148,7 @@ namespace armarx::nav::components
// TODO maybe as optional, but requires some effort
std::unique_ptr<NavigatorRemoteGui> navRemoteGui;
+
};
} // namespace armarx::nav::components
diff --git a/source/Navigation/components/Navigator/RemoteGui.cpp b/source/Navigation/components/Navigator/RemoteGui.cpp
index c07064aa..bddeb4b3 100644
--- a/source/Navigation/components/Navigator/RemoteGui.cpp
+++ b/source/Navigation/components/Navigator/RemoteGui.cpp
@@ -6,6 +6,7 @@
#include "ArmarXCore/core/time/CycleUtil.h"
#include "ArmarXCore/statechart/ParameterMapping.h"
#include "ArmarXCore/util/CPPUtility/trace.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "ArmarXGui/libraries/RemoteGui/WidgetBuilder/IntegerWidgets.h"
#include "ArmarXGui/libraries/RemoteGui/WidgetBuilder/LayoutWidgets.h"
@@ -43,10 +44,7 @@ namespace armarx::nav::components
NavigatorRemoteGui::~NavigatorRemoteGui()
{
- if (runningTask->isRunning())
- {
- runningTask->stop();
- }
+ shutdown();
}
void NavigatorRemoteGui::createRemoteGuiTab()
@@ -269,10 +267,32 @@ namespace armarx::nav::components
ARMARX_TRACE;
- navigator.moveTo(waypoints,
- core::NavigationFramesMap.to_name(core::NavigationFrames::Absolute),
- "NavigatorRemoteGui");
+ try
+ {
+ navigator.moveTo(
+ waypoints,
+ core::NavigationFramesMap.to_name(core::NavigationFrames::Absolute),
+ "NavigatorRemoteGui");
+ }
+ catch (const armarx::LocalException& ex)
+ {
+ ARMARX_WARNING << LogSender::eYellow
+ << "Failed moving to target. Reason: " << GetHandledExceptionString()
+ << LogSender::eReset;
+ }
};
+
+ if (tab.controlGroup.pauseButton.wasClicked())
+ {
+ ARMARX_INFO << "pauseMovement() from RemoteGUI requested";
+ navigator.pauseMovement();
+ }
+
+ if (tab.controlGroup.continueButton.wasClicked())
+ {
+ ARMARX_INFO << "resumeMovement() from RemoteGUI requested";
+ navigator.resumeMovement();
+ }
}
void NavigatorRemoteGui::shutdown()
diff --git a/source/Navigation/libraries/algorithms/CMakeLists.txt b/source/Navigation/libraries/algorithms/CMakeLists.txt
index 0e499126..c4ada721 100644
--- a/source/Navigation/libraries/algorithms/CMakeLists.txt
+++ b/source/Navigation/libraries/algorithms/CMakeLists.txt
@@ -15,12 +15,18 @@ armarx_add_library(
./astar/AStarPlanner.cpp
./astar/Node.cpp
./astar/Planner2D.cpp
+ # smoothing
+ ./smoothing/ChainApproximation.cpp
+ ./smoothing/CircularPathSmoothing.cpp
HEADERS
./algorithms.h
# A*
./astar/AStarPlanner.h
./astar/Node.h
./astar/Planner2D.h
+ # smoothing
+ ./smoothing/ChainApproximation.h
+ ./smoothing/CircularPathSmoothing.h
)
add_library(
diff --git a/source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp b/source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp
index 5e38e8a8..3c282fb1 100644
--- a/source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp
+++ b/source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp
@@ -200,9 +200,11 @@ namespace armarx::nav::algo::astar
createUniformGrid();
ARMARX_INFO << "Setting start node";
NodePtr nodeStart = closestNode(start);
+ ARMARX_CHECK(fulfillsConstraints(nodeStart)) << "Start node in collision!";
ARMARX_INFO << "Setting goal node";
NodePtr nodeGoal = closestNode(goal);
+ ARMARX_CHECK(fulfillsConstraints(nodeGoal)) << "Goal node in collision!";
std::vector<NodePtr> closedSet;
std::vector<NodePtr> openSet;
diff --git a/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.cpp b/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.cpp
index db730742..32899a47 100644
--- a/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.cpp
+++ b/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.cpp
@@ -8,7 +8,7 @@
#include "ArmarXCore/core/exceptions/LocalException.h"
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-namespace armarx
+namespace armarx::nav::algo
{
ChainApproximation::ChainApproximation(const Points& points, const Params& params) :
@@ -211,4 +211,4 @@ namespace armarx
return str;
}
-} // namespace armarx
+} // namespace armarx::nav::algo
diff --git a/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.h b/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.h
index 98448a13..e3673f3f 100644
--- a/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.h
+++ b/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.h
@@ -24,7 +24,7 @@
#include <Eigen/Core>
-namespace armarx
+namespace armarx::nav::algo
{
namespace detail
@@ -95,4 +95,4 @@ namespace armarx
const Params params;
};
-} // namespace armarx
+} // namespace armarx::nav::algo
diff --git a/source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.cpp b/source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.cpp
new file mode 100644
index 00000000..6785a034
--- /dev/null
+++ b/source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.cpp
@@ -0,0 +1,82 @@
+#include "CircularPathSmoothing.h"
+
+namespace armarx::nav::algo
+{
+ // TODO move to Simox
+ float getAngleBetweenVectors(const Eigen::Vector2f& v1, const Eigen::Vector2f& v2)
+ {
+ // Returns an angle between 0 and 180 degree (the minimum angle between the two vectors)
+ const Eigen::Vector2f v1_vec_normed = v1.normalized();
+ const Eigen::Vector2f v2_vec_normed = v2.normalized();
+ const float dot_product_vec =
+ v1_vec_normed(0) * v2_vec_normed(0) + v1_vec_normed(1) * v2_vec_normed(1);
+ float yaw = acos(dot_product_vec);
+ return yaw;
+ }
+
+ CircularPathSmoothing::Points CircularPathSmoothing::smooth(const Points& p)
+ {
+
+ Points points;
+
+ points.push_back(p.front());
+
+ // omit start and end
+ for (size_t i = 1; i < (p.size() - 1); i++)
+ {
+ const Eigen::Vector2f& prev = p.at(i - 1);
+ const Eigen::Vector2f& at = p.at(i);
+ const Eigen::Vector2f& next = p.at(i + 1);
+
+ // the largest radius for smoothing
+ const float maxRadius = std::min((prev - at).norm(), (next - at).norm());
+
+ const float radius = std::min(maxRadius, 300.F);
+
+ const Eigen::Vector2f vIn = (prev - at).normalized();
+ const Eigen::Vector2f vOut = (next - at).normalized();
+
+ // const float phiIn = ::math::Helpers::Angle(vIn);
+ // const float phiOut = ::math::Helpers::Angle(vOut);
+
+ // const float phi = phiIn + (phiOut - phiIn) / 2;
+
+ // the angle between the lines
+ const float phi = getAngleBetweenVectors(vIn, vOut);
+
+ // const Eigen::Vector2f at_dir = Eigen::Rotation2Df(phi) * Eigen::Vector2f::UnitX();
+
+ // const float rho = - sign(v_in.y()*v_out.x() - v_in.x() * v_out.y());
+ // const float chi_0 = std::atan2(v_in.y(), v_in.x());
+ // const float chi_end = std::atan2(v_out.y(), v_out.x());
+
+ // const float l = radius * std::cos(phi / 2);
+ const float l = radius / std::tan(phi / 2);
+
+ points.push_back(at + Eigen::Vector2f(prev - at).normalized() * l);
+ // points.push_back(at);
+ points.push_back(at + Eigen::Vector2f(next - at).normalized() * l);
+
+ // Eigen::Vector2f dirAtToPrev = (prev - at).normalized();
+ // Eigen::Vector2f dirAtToNext = (next - at).normalized();
+
+ // Eigen::Vector2f center =
+ // at + dirAtToPrev * l +
+ // radius * Eigen::Vector2f(dirAtToPrev.y(), -dirAtToPrev.x());
+
+ // const float alpha_start =
+ // math::Helpers::Angle(Eigen::Vector2f(dirAtToPrev.y(), -dirAtToPrev.x()));
+ // const float alpha_end =
+ // math::Helpers::Angle(Eigen::Vector2f(-dirAtToNext.y(), dirAtToNext.x()));
+
+ // points.push_back(center + radius * Eigen::Vector2f(Eigen::Rotation2Df(alpha_end) * Eigen::Vector2f::UnitX()));
+ // points.push_back(center + radius * Eigen::Vector2f(Eigen::Rotation2Df(alpha_start + (alpha_start - alpha_end)/2) * Eigen::Vector2f::UnitX()));
+ // points.push_back(center + radius * Eigen::Vector2f(Eigen::Rotation2Df(alpha_start) * Eigen::Vector2f::UnitX()));
+ // points.push_back(center);
+ }
+
+ points.push_back(p.back());
+
+ return points;
+ }
+} // namespace armarx::nav::algo
diff --git a/source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.h b/source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.h
index bbf27c97..672f7c2f 100644
--- a/source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.h
+++ b/source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.h
@@ -22,6 +22,9 @@
#pragma once
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
namespace armarx::nav::algo
{
@@ -34,71 +37,7 @@ namespace armarx::nav::algo
using Points = std::vector<Point>;
/// circular path smoothing
- Points smooth(const Points& p)
- {
-
- Points points;
-
- points.push_back(p.front());
-
- // omit start and end
- for (size_t i = 1; i < (p.size() - 1); i++)
- {
- const Eigen::Vector2f& prev = p.at(i - 1);
- const Eigen::Vector2f& at = p.at(i);
- const Eigen::Vector2f& next = p.at(i + 1);
-
- // the largest radius for smoothing
- const float maxRadius = std::min((prev - at).norm(), (next - at).norm());
-
- const float radius = std::min(maxRadius, 300.F);
-
- const Eigen::Vector2f vIn = (prev - at).normalized();
- const Eigen::Vector2f vOut = (next - at).normalized();
-
- // const float phiIn = ::math::Helpers::Angle(vIn);
- // const float phiOut = ::math::Helpers::Angle(vOut);
-
- // const float phi = phiIn + (phiOut - phiIn) / 2;
-
- // the angle between the lines
- const float phi = getAngleBetweenVectors(vIn, vOut);
-
- // const Eigen::Vector2f at_dir = Eigen::Rotation2Df(phi) * Eigen::Vector2f::UnitX();
-
- // const float rho = - sign(v_in.y()*v_out.x() - v_in.x() * v_out.y());
- // const float chi_0 = std::atan2(v_in.y(), v_in.x());
- // const float chi_end = std::atan2(v_out.y(), v_out.x());
-
- // const float l = radius * std::cos(phi / 2);
- const float l = radius / std::tan(phi / 2);
-
- points.push_back(at + Eigen::Vector2f(prev - at).normalized() * l);
- // points.push_back(at);
- points.push_back(at + Eigen::Vector2f(next - at).normalized() * l);
-
- // Eigen::Vector2f dirAtToPrev = (prev - at).normalized();
- // Eigen::Vector2f dirAtToNext = (next - at).normalized();
-
- // Eigen::Vector2f center =
- // at + dirAtToPrev * l +
- // radius * Eigen::Vector2f(dirAtToPrev.y(), -dirAtToPrev.x());
-
- // const float alpha_start =
- // math::Helpers::Angle(Eigen::Vector2f(dirAtToPrev.y(), -dirAtToPrev.x()));
- // const float alpha_end =
- // math::Helpers::Angle(Eigen::Vector2f(-dirAtToNext.y(), dirAtToNext.x()));
-
- // points.push_back(center + radius * Eigen::Vector2f(Eigen::Rotation2Df(alpha_end) * Eigen::Vector2f::UnitX()));
- // points.push_back(center + radius * Eigen::Vector2f(Eigen::Rotation2Df(alpha_start + (alpha_start - alpha_end)/2) * Eigen::Vector2f::UnitX()));
- // points.push_back(center + radius * Eigen::Vector2f(Eigen::Rotation2Df(alpha_start) * Eigen::Vector2f::UnitX()));
- // points.push_back(center);
- }
-
- points.push_back(p.back());
-
- return points;
- }
+ Points smooth(const Points& p);
};
} // namespace armarx::nav::algo
diff --git a/source/Navigation/libraries/client/NavigationStackConfig.cpp b/source/Navigation/libraries/client/NavigationStackConfig.cpp
index b503f003..71e6ca15 100644
--- a/source/Navigation/libraries/client/NavigationStackConfig.cpp
+++ b/source/Navigation/libraries/client/NavigationStackConfig.cpp
@@ -1,8 +1,14 @@
#include "NavigationStackConfig.h"
+#include <memory>
+
+#include "RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h"
+#include "RobotAPI/libraries/aron/core/navigator/data/primitive/String.h"
#include <RobotAPI/interface/aron/Aron.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
#include "Navigation/libraries/core/constants.h"
+#include "Navigation/libraries/core/types.h"
namespace armarx::nav::client
{
@@ -20,12 +26,13 @@ namespace armarx::nav::client
glob_plan::AlgorithmNames.to_name(params.algorithm())));
element->addElement(core::PARAMS_KEY, params.toAron());
- dict.addElement(core::GLOBAL_PLANNER, element);
+ dict.addElement(core::StackLayerNames.to_name(core::StackLayer::GlobalPlanner), element);
return *this;
}
- NavigationStackConfig& NavigationStackConfig::configureLocalPlanner(const loc_plan::LocalPlannerParams& params)
+ NavigationStackConfig&
+ NavigationStackConfig::configureLocalPlanner(const loc_plan::LocalPlannerParams& params)
{
aron::datanavigator::DictNavigatorPtr element(new aron::datanavigator::DictNavigator);
element->addElement(core::NAME_KEY,
@@ -33,12 +40,13 @@ namespace armarx::nav::client
loc_plan::AlgorithmNames.to_name(params.algorithm())));
element->addElement(core::PARAMS_KEY, params.toAron());
- dict.addElement(core::LOCAL_PLANNER, element);
+ dict.addElement(core::StackLayerNames.to_name(core::StackLayer::LocalPlanner), element);
return *this;
}
- NavigationStackConfig& NavigationStackConfig::configureTrajectoryController(const traj_ctrl::TrajectoryControllerParams& params)
+ NavigationStackConfig& NavigationStackConfig::configureTrajectoryController(
+ const traj_ctrl::TrajectoryControllerParams& params)
{
aron::datanavigator::DictNavigatorPtr element(new aron::datanavigator::DictNavigator);
element->addElement(core::NAME_KEY,
@@ -46,12 +54,14 @@ namespace armarx::nav::client
traj_ctrl::AlgorithmNames.to_name(params.algorithm())));
element->addElement(core::PARAMS_KEY, params.toAron());
- dict.addElement(core::TRAJECTORY_CONTROLLER, element);
+ dict.addElement(core::StackLayerNames.to_name(core::StackLayer::TrajectoryController),
+ element);
return *this;
}
- NavigationStackConfig& NavigationStackConfig::configureSafetyController(const safe_ctrl::SafetyControllerParams& params)
+ NavigationStackConfig& NavigationStackConfig::configureSafetyController(
+ const safe_ctrl::SafetyControllerParams& params)
{
aron::datanavigator::DictNavigatorPtr element(new aron::datanavigator::DictNavigator);
element->addElement(core::NAME_KEY,
@@ -59,7 +69,7 @@ namespace armarx::nav::client
safe_ctrl::AlgorithmNames.to_name(params.algorithm())));
element->addElement(core::PARAMS_KEY, params.toAron());
- dict.addElement(core::SAFETY_CONTROLLER, element);
+ dict.addElement(core::StackLayerNames.to_name(core::StackLayer::SafetyController), element);
return *this;
}
diff --git a/source/Navigation/libraries/client/NavigationStackConfig.h b/source/Navigation/libraries/client/NavigationStackConfig.h
index 84bd4b46..6f75af5d 100644
--- a/source/Navigation/libraries/client/NavigationStackConfig.h
+++ b/source/Navigation/libraries/client/NavigationStackConfig.h
@@ -24,18 +24,16 @@
#include <memory>
-#include "RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h"
-#include "RobotAPI/libraries/aron/core/navigator/data/primitive/String.h"
-#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/interface/aron/Aron.h>
#include "Navigation/libraries/core/types.h"
-#include "Navigation/libraries/factories/NavigationStackFactory.h"
+
+// TODO remove these headers once ARON types are used as params
#include "Navigation/libraries/global_planning/GlobalPlanner.h"
#include "Navigation/libraries/local_planning/LocalPlanner.h"
#include "Navigation/libraries/safety_control/SafetyController.h"
#include "Navigation/libraries/trajectory_control/TrajectoryController.h"
-#include <type_traits>
namespace armarx::nav::client
{
@@ -56,7 +54,6 @@ namespace armarx::nav::client
core::NavigationFrames navigationFrame = core::NavigationFrames::Absolute;
virtual aron::datanavigator::DictNavigatorPtr toAron() const;
-
};
class NavigationStackConfig
diff --git a/source/Navigation/libraries/core/Trajectory.cpp b/source/Navigation/libraries/core/Trajectory.cpp
index b5a8d5de..d7d7168b 100644
--- a/source/Navigation/libraries/core/Trajectory.cpp
+++ b/source/Navigation/libraries/core/Trajectory.cpp
@@ -1,16 +1,46 @@
#include "Trajectory.h"
#include <algorithm>
+#include <cmath>
+#include <cstddef>
#include <iterator>
#include <limits>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <wykobi.hpp>
+
+#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/math/LinearInterpolatedPose.h>
+#include "Navigation/libraries/conversions/eigen.h"
#include "Navigation/libraries/core/types.h"
-#include "wykobi.hpp"
namespace armarx::nav::core
{
+
+ namespace conv
+ {
+
+ TrajectoryPoint toTrajectoryPoint(const Pose& pose)
+ {
+ return TrajectoryPoint{.waypoint = {.pose = pose}, .velocity = 0.F};
+ }
+
+ TrajectoryPoints toTrajectoryPoints(const Path& path)
+ {
+ TrajectoryPoints trajectoryPoints;
+ trajectoryPoints.reserve(path.size());
+
+ std::transform(
+ path.begin(), path.end(), std::back_inserter(trajectoryPoints), toTrajectoryPoint);
+
+ return trajectoryPoints;
+ }
+
+ } // namespace conv
+
Projection Trajectory::getProjection(const Pose& pose,
const VelocityInterpolations& velocityInterpolation) const
{
@@ -82,4 +112,45 @@ namespace armarx::nav::core
return positions;
}
+
+ Trajectory Trajectory::FromPath(const Path& path)
+ {
+ return {conv::toTrajectoryPoints(path)};
+ }
+
+ Trajectory Trajectory::FromPath(const Pose& start, const Positions& waypoints, const Pose& goal)
+ {
+ // currently, only 2D version
+
+ Path path;
+ path.reserve(waypoints.size());
+
+ if (waypoints.empty())
+ {
+ return FromPath({start, goal});
+ }
+
+ const auto directedPose = [](const Position & position, const Position & target) -> Pose
+ {
+ const Direction direction = target - position;
+
+ const float yaw = math::Helpers::Angle(nav::conv::to2D(direction));
+
+ // our robot is moving into y direction ( y is forward ... )
+ const Eigen::Rotation2Df R = Eigen::Rotation2Df(yaw) * Eigen::Rotation2Df(-M_PI_2f32);
+
+ return nav::conv::to3D(Eigen::Translation2f(nav::conv::to2D(position)) * R);
+ };
+
+ // insert waypoints pointing to next waypoint
+ for (size_t i = 0; i < waypoints.size() - 1; i++)
+ {
+ path.emplace_back(directedPose(waypoints.at(i), waypoints.at(i + 1)));
+ }
+
+ path.emplace_back(directedPose(waypoints.back(), goal.translation()));
+ path.push_back(goal);
+
+ return FromPath(path);
+ }
} // namespace armarx::nav::core
diff --git a/source/Navigation/libraries/core/Trajectory.h b/source/Navigation/libraries/core/Trajectory.h
index 59e6d4ca..b875ba8f 100644
--- a/source/Navigation/libraries/core/Trajectory.h
+++ b/source/Navigation/libraries/core/Trajectory.h
@@ -22,6 +22,7 @@
#pragma once
+#include <memory>
#include "Navigation/libraries/core/types.h"
namespace armarx::nav::core
@@ -33,6 +34,8 @@ namespace armarx::nav::core
float velocity;
};
+ using TrajectoryPoints = std::vector<TrajectoryPoint>;
+
struct Projection
{
TrajectoryPoint projection;
@@ -47,6 +50,10 @@ namespace armarx::nav::core
LastWaypoint
};
+ class Trajectory;
+ using TrajectoryPtr = std::shared_ptr<Trajectory>;
+
+
class Trajectory
{
public:
@@ -59,10 +66,19 @@ namespace armarx::nav::core
[[nodiscard]] std::vector<Eigen::Vector3f> positions() const;
+ //! Note: the velocity will not be set!
+ // currently, only 2D version
+ // y is pointing forward
+ static Trajectory FromPath(const Path& path);
+
+ //! Note: the velocity will not be set!
+ // currently, only 2D version
+ // y is pointing forward
+ static Trajectory FromPath(const Pose& start, const Positions& waypoints, const Pose& goal);
+
private:
std::vector<TrajectoryPoint> points;
};
- using TrajectoryPtr = std::shared_ptr<Trajectory>;
} // namespace armarx::nav::core
diff --git a/source/Navigation/libraries/core/constants.h b/source/Navigation/libraries/core/constants.h
index e904f386..192b440d 100644
--- a/source/Navigation/libraries/core/constants.h
+++ b/source/Navigation/libraries/core/constants.h
@@ -22,16 +22,28 @@
#pragma once
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
namespace armarx::nav::core
{
- const std::string NAME_KEY = "name";
- const std::string PARAMS_KEY = "params";
+ const inline std::string NAME_KEY = "name";
+ const inline std::string PARAMS_KEY = "params";
+
+ enum class StackLayer
+ {
+ GlobalPlanner,
+ LocalPlanner,
+ TrajectoryController,
+ SafetyController
+ };
+
+ const inline simox::meta::EnumNames<StackLayer> StackLayerNames
+ {
+ {StackLayer::GlobalPlanner, "GlobalPlanner"},
+ {StackLayer::LocalPlanner, "LocalPlanner"},
+ {StackLayer::TrajectoryController, "TrajectoryController"},
+ {StackLayer::SafetyController, "SafetyController"}};
- // TODO enum
- const std::string GLOBAL_PLANNER = "global_planner";
- const std::string LOCAL_PLANNER = "local_planner";
- const std::string TRAJECTORY_CONTROLLER = "trajectory_controller";
- const std::string SAFETY_CONTROLLER = "safety_controller";
} // namespace armarx::nav::core
diff --git a/source/Navigation/libraries/core/fwd.h b/source/Navigation/libraries/core/fwd.h
new file mode 100644
index 00000000..75cacf99
--- /dev/null
+++ b/source/Navigation/libraries/core/fwd.h
@@ -0,0 +1,33 @@
+/*
+ * 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
+
+namespace armarx::nav::core
+{
+ struct Scene;
+ struct StaticScene;
+ struct DynamicScene;
+
+ struct Waypoint;
+ struct Twist;
+
+} // namespace armarx::nav::core
diff --git a/source/Navigation/libraries/core/types.h b/source/Navigation/libraries/core/types.h
index 4fa5945a..123916ff 100644
--- a/source/Navigation/libraries/core/types.h
+++ b/source/Navigation/libraries/core/types.h
@@ -50,6 +50,7 @@ namespace armarx::nav::core
using Pose = Eigen::Isometry3f;
using Pose2D = Eigen::Isometry2f;
+ using Position = Eigen::Vector3f;
using Direction = Eigen::Vector3f;
using Rotation = Eigen::Matrix3f;
@@ -57,6 +58,9 @@ namespace armarx::nav::core
using LinearVelocity = Eigen::Vector3f;
using AngularVelocity = Eigen::Vector3f;
+ using Path = std::vector<Pose>;
+ using Positions = std::vector<Position>;
+
struct Twist
{
LinearVelocity linear;
diff --git a/source/Navigation/libraries/factories/GlobalPlannerFactory.h b/source/Navigation/libraries/factories/GlobalPlannerFactory.h
index 9c76caad..79f13300 100644
--- a/source/Navigation/libraries/factories/GlobalPlannerFactory.h
+++ b/source/Navigation/libraries/factories/GlobalPlannerFactory.h
@@ -22,7 +22,10 @@
#pragma once
-#include "Navigation/libraries/global_planning/GlobalPlanner.h"
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+
+#include "Navigation/libraries/core/fwd.h"
+#include "Navigation/libraries/factories/fwd.h"
namespace armarx::nav::fac
{
diff --git a/source/Navigation/libraries/factories/LocalPlannerFactory.h b/source/Navigation/libraries/factories/LocalPlannerFactory.h
index 1b1d944f..e6bb7b5f 100644
--- a/source/Navigation/libraries/factories/LocalPlannerFactory.h
+++ b/source/Navigation/libraries/factories/LocalPlannerFactory.h
@@ -22,9 +22,11 @@
#pragma once
-#include "RobotAPI/libraries/aron/core/navigator/data/container/Dict.h"
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+
+#include "Navigation/libraries/core/fwd.h"
+#include "Navigation/libraries/factories/fwd.h"
-#include "Navigation/libraries/local_planning/LocalPlanner.h"
namespace armarx::nav::fac
{
diff --git a/source/Navigation/libraries/factories/NavigationStackFactory.cpp b/source/Navigation/libraries/factories/NavigationStackFactory.cpp
index 037b52fc..95bac2d7 100644
--- a/source/Navigation/libraries/factories/NavigationStackFactory.cpp
+++ b/source/Navigation/libraries/factories/NavigationStackFactory.cpp
@@ -16,8 +16,10 @@ namespace armarx::nav::fac
using aron::datanavigator::DictNavigator;
const auto getElementOrNull =
- [¶ms](const std::string & key) -> DictNavigator::PointerType
+ [¶ms](const core::StackLayer & layer) -> DictNavigator::PointerType
{
+ const std::string key = core::StackLayerNames.to_name(layer);
+
if (params->hasElement(key))
{
return DictNavigator::DynamicCast(params->getElement(key));
@@ -28,10 +30,10 @@ namespace armarx::nav::fac
return nullptr;
};
- const auto globalPlannerCfg = getElementOrNull(core::GLOBAL_PLANNER);
- const auto localPlannerCfg = getElementOrNull(core::LOCAL_PLANNER);
- const auto trajectoryControllerCfg = getElementOrNull(core::TRAJECTORY_CONTROLLER);
- const auto safetyControllerCfg = getElementOrNull(core::SAFETY_CONTROLLER);
+ const auto globalPlannerCfg = getElementOrNull(core::StackLayer::GlobalPlanner);
+ const auto localPlannerCfg = getElementOrNull(core::StackLayer::LocalPlanner);
+ const auto trajectoryControllerCfg = getElementOrNull(core::StackLayer::TrajectoryController);
+ const auto safetyControllerCfg = getElementOrNull(core::StackLayer::SafetyController);
return server::NavigationStack
{
diff --git a/source/Navigation/libraries/factories/NavigationStackFactory.h b/source/Navigation/libraries/factories/NavigationStackFactory.h
index d7e7a8d4..6f1abf59 100644
--- a/source/Navigation/libraries/factories/NavigationStackFactory.h
+++ b/source/Navigation/libraries/factories/NavigationStackFactory.h
@@ -23,6 +23,10 @@
#pragma once
#include "Navigation/libraries/server/NavigationStack.h"
+#include "RobotAPI/libraries/aron/core/navigator/data/container/Dict.h"
+
+#include "Navigation/libraries/core/fwd.h"
+
namespace armarx::nav::fac
{
/**
diff --git a/source/Navigation/libraries/factories/SafetyControllerFactory.h b/source/Navigation/libraries/factories/SafetyControllerFactory.h
index c31bdebc..2b9bb629 100644
--- a/source/Navigation/libraries/factories/SafetyControllerFactory.h
+++ b/source/Navigation/libraries/factories/SafetyControllerFactory.h
@@ -22,9 +22,10 @@
#pragma once
-#include "RobotAPI/libraries/aron/core/navigator/data/container/Dict.h"
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-#include "Navigation/libraries/safety_control/SafetyController.h"
+#include "Navigation/libraries/core/fwd.h"
+#include "Navigation/libraries/factories/fwd.h"
namespace armarx::nav::fac
{
diff --git a/source/Navigation/libraries/factories/TrajectoryControllerFactory.h b/source/Navigation/libraries/factories/TrajectoryControllerFactory.h
index d00ac6b5..cd12eeb8 100644
--- a/source/Navigation/libraries/factories/TrajectoryControllerFactory.h
+++ b/source/Navigation/libraries/factories/TrajectoryControllerFactory.h
@@ -22,10 +22,10 @@
#pragma once
-#include "RobotAPI/libraries/aron/core/navigator/data/container/Dict.h"
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-#include "Navigation/libraries/core/types.h"
-#include "Navigation/libraries/trajectory_control/TrajectoryController.h"
+#include "Navigation/libraries/core/fwd.h"
+#include "Navigation/libraries/factories/fwd.h"
namespace armarx::nav::fac
{
diff --git a/source/Navigation/libraries/factories/fwd.h b/source/Navigation/libraries/factories/fwd.h
new file mode 100644
index 00000000..adb54ba3
--- /dev/null
+++ b/source/Navigation/libraries/factories/fwd.h
@@ -0,0 +1,53 @@
+/*
+ * 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 <memory>
+
+namespace armarx::nav
+{
+
+ namespace glob_plan
+ {
+ class GlobalPlanner;
+ using GlobalPlannerPtr = std::shared_ptr<GlobalPlanner>;
+ } // namespace glob_plan
+
+ namespace loc_plan
+ {
+ class LocalPlanner;
+ using LocalPlannerPtr = std::shared_ptr<LocalPlanner>;
+ } // namespace loc_plan
+
+ namespace traj_ctrl
+ {
+ class TrajectoryController;
+ using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
+ } // namespace traj_ctrl
+
+ namespace safe_ctrl
+ {
+ class SafetyController;
+ using SafetyControllerPtr = std::shared_ptr<SafetyController>;
+ } // namespace safe_ctrl
+
+} // namespace armarx::nav
diff --git a/source/Navigation/libraries/global_planning/AStar.cpp b/source/Navigation/libraries/global_planning/AStar.cpp
index 523e9f83..59e55675 100644
--- a/source/Navigation/libraries/global_planning/AStar.cpp
+++ b/source/Navigation/libraries/global_planning/AStar.cpp
@@ -1,6 +1,15 @@
#include "AStar.h"
-#include "Navigation/libraries/algorithms/astar/AStarPlanner.h"
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "Navigation/libraries/global_planning/Point2Point.h"
+#include <Navigation/libraries/algorithms/astar/AStarPlanner.h>
+#include <Navigation/libraries/algorithms/smoothing/ChainApproximation.h>
+#include <Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.h>
+#include <Navigation/libraries/conversions/eigen.h>
+#include <Navigation/libraries/core/Trajectory.h>
#include <Navigation/libraries/global_planning/aron/AStarParams.aron.generated.h>
namespace armarx::nav::glob_plan
@@ -30,16 +39,64 @@ namespace armarx::nav::glob_plan
{
}
+ auto robotCollisionModel()
+ {
+ VirtualRobot::CoinVisualizationFactory factory;
+ const auto cylinder = factory.createCylinder(1500.F / 2, 2000.F);
+
+ VirtualRobot::CollisionModelPtr collisionModel(new VirtualRobot::CollisionModel(cylinder));
+
+ return collisionModel;
+ }
+
+ std::vector<Eigen::Vector2f> AStar::postProcessPath(const std::vector<Eigen::Vector2f>& path)
+ {
+ /// chain approximation
+ algo::ChainApproximation approx(path,
+ algo::ChainApproximation::Params{.distanceTh = 200.F});
+ approx.approximate();
+ const auto p = approx.approximatedChain();
+
+ // visualizePath(p, "approximated", simox::Color::green());
+
+ algo::CircularPathSmoothing smoothing;
+ const auto points = smoothing.smooth(p);
+
+ return points;
+ }
+
core::TrajectoryPtr AStar::plan(const core::Pose& goal)
{
+ ARMARX_TRACE;
+
algo::astar::AStarPlanner planner(scene.robot, scene.staticScene->objects, 100.f);
// planner.setRobotColModel("Platform-colmodel");
- // TODO!
- // planner.setRobotCollisionModel(robotCollisionModel());
+ // planner.setRobotColModel("Platform-colmodel");
+ planner.setRobotCollisionModel(robotCollisionModel());
+
+ const Eigen::Vector2f startPos = conv::to2D(scene.robot->getGlobalPosition());
+ const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
+
+ const auto timeStart = IceUtil::Time::now();
+ const auto plan = planner.plan(startPos, goalPos);
+ const auto timeEnd = IceUtil::Time::now();
+
+ ARMARX_IMPORTANT << "A* planning took " << (timeEnd - timeStart).toMilliSeconds() << " ms";
+ ARMARX_IMPORTANT << "Path contains " << plan.size() << " points";
+
+ auto smoothPlan = postProcessPath(plan);
+ ARMARX_IMPORTANT << "Smooth path contains " << smoothPlan.size() << " points";
+
+ // we need to strip the first and the last points from the plan as they encode the start and goal position
+ smoothPlan.erase(plan.begin());
+ smoothPlan.pop_back();
- // TODO
- return {};
+ ARMARX_TRACE;
+ auto trajectory = core::Trajectory::FromPath(
+ core::Pose(scene.robot->getGlobalPose()), conv::to3D(smoothPlan), goal);
+ ARMARX_TRACE;
+ return std::make_shared<core::Trajectory>(trajectory);
}
} // namespace armarx::nav::glob_plan
diff --git a/source/Navigation/libraries/global_planning/AStar.h b/source/Navigation/libraries/global_planning/AStar.h
index 5a78af3d..42bfa012 100644
--- a/source/Navigation/libraries/global_planning/AStar.h
+++ b/source/Navigation/libraries/global_planning/AStar.h
@@ -45,9 +45,14 @@ namespace armarx::nav::glob_plan
{
public:
AStar(const AStarParams& params, const core::Scene& ctx);
+ ~AStar() override = default;
core::TrajectoryPtr plan(const core::Pose& goal) override;
+ protected:
+ std::vector<Eigen::Vector2f> postProcessPath(const std::vector<Eigen::Vector2f>& path);
+
+ private:
AStarParams params;
};
diff --git a/source/Navigation/libraries/global_planning/GlobalPlanner.h b/source/Navigation/libraries/global_planning/GlobalPlanner.h
index 216e5167..927600d7 100644
--- a/source/Navigation/libraries/global_planning/GlobalPlanner.h
+++ b/source/Navigation/libraries/global_planning/GlobalPlanner.h
@@ -30,7 +30,7 @@
#include "Navigation/libraries/core/StaticScene.h"
#include "Navigation/libraries/core/Trajectory.h"
#include "Navigation/libraries/core/types.h"
-#include "core.h"
+#include "Navigation/libraries/global_planning/core.h"
namespace armarx::nav::glob_plan
{
diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp
index 3a2e0fdc..193caad2 100644
--- a/source/Navigation/libraries/server/Navigator.cpp
+++ b/source/Navigation/libraries/server/Navigator.cpp
@@ -11,12 +11,19 @@
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "monitoring/GoalReachedMonitor.h"
+#include "Navigation/libraries/server/monitoring/GoalReachedMonitor.h"
#include <Navigation/libraries/core/types.h>
#include <Navigation/libraries/server/StackResult.h>
namespace armarx::nav::server
{
+ void stopIfRunning(PeriodicTask<Navigator>::pointer_type& task)
+ {
+ if (task)
+ {
+ task->stop();
+ }
+ }
Navigator::Navigator(const NavigatorConfig& config) : config(config)
{
@@ -28,6 +35,13 @@ namespace armarx::nav::server
{
ARMARX_INFO << "Navigator destructor";
pauseMovement();
+
+ stopIfRunning(localPlannerTask);
+ stopIfRunning(trajectoryControllerTask);
+ stopIfRunning(safetyControllerTask);
+ stopIfRunning(executorTask);
+ stopIfRunning(introspectorTask);
+ stopIfRunning(monitorTask);
}
void Navigator::moveTo(const std::vector<core::Pose>& waypoints,
@@ -62,6 +76,8 @@ namespace armarx::nav::server
// TODO plan on multiple waypoints
res.globalTrajectory = config.stack.globalPlanner->plan(core::Pose(waypoints.at(0)));
+ ARMARX_INFO << "Global planning completed. Will now start all required threads";
+
// local planner
if (config.stack.localPlanner)
{
@@ -69,7 +85,9 @@ namespace armarx::nav::server
localPlannerTask =
new PeriodicTask<Navigator>(this,
&Navigator::updateLocalPlanner,
- config.general.tasks.localPlanningUpdatePeriod);
+ config.general.tasks.localPlanningUpdatePeriod,
+ false,
+ "LocalPlannerTask");
localPlannerTask->start();
}
@@ -78,7 +96,9 @@ namespace armarx::nav::server
trajectoryControllerTask =
new PeriodicTask<Navigator>(this,
&Navigator::updateTrajectoryController,
- config.general.tasks.trajectoryControllerUpdatePeriod);
+ config.general.tasks.trajectoryControllerUpdatePeriod,
+ false,
+ "TrajectoryControllerTask");
trajectoryControllerTask->start();
// safety controller
@@ -88,26 +108,41 @@ namespace armarx::nav::server
safetyControllerTask =
new PeriodicTask<Navigator>(this,
&Navigator::updateSafetyController,
- config.general.tasks.safetyControllerUpdatePeriod);
+ config.general.tasks.safetyControllerUpdatePeriod,
+ false,
+ "SafetyControllerTask");
safetyControllerTask->start();
}
// executor
- executorTask = new PeriodicTask<Navigator>(
- this, &Navigator::updateExecutor, config.general.tasks.executorUpdatePeriod);
+ 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);
+ introspectorTask =
+ new PeriodicTask<Navigator>(this,
+ &Navigator::updateIntrospector,
+ config.general.tasks.introspectorUpdatePeriod,
+ false,
+ "IntrospectorTask");
introspectorTask->start();
// monitoring
goalReachedMonitor =
GoalReachedMonitor(waypoints, *config.scene, GoalReachedMonitorConfig());
- monitorTask = new PeriodicTask<Navigator>(
- this, &Navigator::updateMonitor, config.general.tasks.monitorUpdatePeriod);
+ monitorTask = new PeriodicTask<Navigator>(this,
+ &Navigator::updateMonitor,
+ config.general.tasks.monitorUpdatePeriod,
+ false,
+ "MonitorTask");
monitorTask->start();
+
+ // could be false if pauseMovement has been called in the past
+ movementEnabled.store(true);
}
void Navigator::moveTowards(const core::Direction& direction,
@@ -180,20 +215,20 @@ namespace armarx::nav::server
void Navigator::updateMonitor()
{
- if (goalReachedMonitor->goalReached())
+ if (goalReachedMonitor->goalReached() and movementEnabled.load())
{
// [[unlikely]]
ARMARX_INFO << "Goal reached!";
movementEnabled.store(false);
- // stop all threads, including this one
- localPlannerTask->stop();
- trajectoryControllerTask->stop();
- safetyControllerTask->stop();
- executorTask->stop();
- introspectorTask->stop();
- monitorTask->stop();
+ // stop all threads, excluding this one
+ stopIfRunning(localPlannerTask);
+ stopIfRunning(trajectoryControllerTask);
+ stopIfRunning(safetyControllerTask);
+ stopIfRunning(executorTask);
+ stopIfRunning(introspectorTask);
+ // monitorTask->stop();
}
}
@@ -248,6 +283,8 @@ namespace armarx::nav::server
{
movementEnabled.store(false);
const core::Twist zero{Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()};
+
+ ARMARX_CHECK_NOT_NULL(config.executor);
config.executor->move(zero);
}
@@ -256,6 +293,12 @@ namespace armarx::nav::server
movementEnabled.store(true);
}
+ bool Navigator::isActive() const noexcept
+ {
+ // TODO maybe need to check other stuff
+ return movementEnabled;
+ }
+
Navigator::Navigator(Navigator&& other) noexcept :
config{std::move(other.config)}, movementEnabled{other.movementEnabled.load()}
{
diff --git a/source/Navigation/libraries/server/Navigator.h b/source/Navigation/libraries/server/Navigator.h
index 514d5976..9a5852fc 100644
--- a/source/Navigation/libraries/server/Navigator.h
+++ b/source/Navigation/libraries/server/Navigator.h
@@ -83,6 +83,8 @@ namespace armarx::nav::server
void pauseMovement();
void resumeMovement();
+ bool isActive() const noexcept;
+
// // Non-API
Navigator(Navigator&& other) noexcept;
virtual ~Navigator();
diff --git a/source/Navigation/libraries/server/monitoring/GoalReachedMonitor.cpp b/source/Navigation/libraries/server/monitoring/GoalReachedMonitor.cpp
index d044f43a..d7c70957 100644
--- a/source/Navigation/libraries/server/monitoring/GoalReachedMonitor.cpp
+++ b/source/Navigation/libraries/server/monitoring/GoalReachedMonitor.cpp
@@ -1,15 +1,14 @@
#include "GoalReachedMonitor.h"
#include <VirtualRobot/Robot.h>
-
-#include <exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::nav::server
{
GoalReachedMonitor::GoalReachedMonitor(const std::vector<core::Pose>& waypoints,
- const core::Scene& scene,
- const GoalReachedMonitorConfig& config) :
+ const core::Scene& scene,
+ const GoalReachedMonitorConfig& config) :
waypoints(waypoints), scene(scene), config(config)
{
ARMARX_CHECK(not waypoints.empty());
--
GitLab