From 0b22bbd50e1451dd334d3b6d9bf68f28023c244f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 8 Jul 2021 11:55:31 +0200
Subject: [PATCH] update
---
.../components/Navigator/Navigator.cpp | 8 ++
.../Navigator/NavigatorInterface.ice | 4 +-
.../smoothing/CircularPathSmoothing.h | 103 ++++++++++++++++++
3 files changed, 113 insertions(+), 2 deletions(-)
create mode 100644 source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.h
diff --git a/source/Navigation/components/Navigator/Navigator.cpp b/source/Navigation/components/Navigator/Navigator.cpp
index 27c79825..e60cc68e 100644
--- a/source/Navigation/components/Navigator/Navigator.cpp
+++ b/source/Navigation/components/Navigator/Navigator.cpp
@@ -21,6 +21,7 @@
*/
#include "Navigator.h"
+#include "ArmarXCore/core/services/tasks/TaskUtil.h"
// Navigation
#include "Navigation/libraries/core/types.h"
@@ -67,10 +68,17 @@ namespace armarx::nav::components
const std::string& navigationMode,
const Ice::Current&)
{
+ updateContext();
server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);
server::Navigator navigator{stack, ctx};
+ SimplePeriodicTask<> task(
+ [](){navigator.updateContext()}
+ );
+ task.start()
+
+
navigator.moveTo(core::Pose(pose), core::NavigationFramesMap.from_name(navigationMode));
}
diff --git a/source/Navigation/components/Navigator/NavigatorInterface.ice b/source/Navigation/components/Navigator/NavigatorInterface.ice
index 4b422a6a..94c689e6 100644
--- a/source/Navigation/components/Navigator/NavigatorInterface.ice
+++ b/source/Navigation/components/Navigator/NavigatorInterface.ice
@@ -38,8 +38,8 @@ module armarx
interface NavigatorInterface
{
- void moveTo(Eigen::Matrix4f pose, aron::data::AronDict config, string navigationFrame);
- void moveTowards(Eigen::Matrix4f direction, aron::data::AronDict config, string navigationFrame);
+ void moveTo(Eigen::Matrix4fSeq waypoints, aron::data::AronDict config, string navigationFrame);
+ void moveTowards(Eigen::Vector3f direction, aron::data::AronDict config, string navigationFrame);
};
};
diff --git a/source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.h b/source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.h
new file mode 100644
index 00000000..403c5470
--- /dev/null
+++ b/source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.h
@@ -0,0 +1,103 @@
+/*
+ * 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::algo
+ {
+
+class CircularPathSmoothing
+ {
+ public:
+ // TODO(fabian.reister): add param
+
+ using Point = Eigen::Vector2f;
+ 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;
+ }
+ };
+
+ } // namespace armarx::nav::algo
\ No newline at end of file
--
GitLab