diff --git a/.clang-tidy b/.clang-tidy
index fc2f7fe90f6696c438dbc6090d06f2c74ce8d02e..6e80b9026b3eb65a5417ca390eac4ccc1903e85e 100644
--- a/.clang-tidy
+++ b/.clang-tidy
@@ -67,7 +67,7 @@ CheckOptions:
- key: readability-identifier-naming.ConstantMemberPrefix
value: ''
- key: readability-identifier-naming.ConstantMemberCase
- value: UPPER_CASE
+ value: camelBack
- key: readability-identifier-naming.ConstantParameterPrefix
value: ''
- key: readability-identifier-naming.ConstantParameterCase
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
index 0386c314dea9df3f0b609fe0eb022a7cf380432b..0213bd3197be6c17e3ea23a5eb0666a76f3a2558 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
@@ -124,8 +124,8 @@ namespace armarx::navigation::algorithm::astar
ARMARX_CHECK_NOT_NULL(VirtualRobot::CollisionChecker::getGlobalCollisionChecker());
ARMARX_CHECK_NOT_NULL(obstacles);
- const core::Pose globalPose = Eigen::Translation3f(conv::to3D(n->position)) *
- Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX());
+ const core::Pose globalPose(Eigen::Translation3f(conv::to3D(n->position))); //*
+ //Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX());
robotCollisionModel->setGlobalPose(globalPose.matrix());
@@ -186,6 +186,7 @@ namespace armarx::navigation::algorithm::astar
// << "No collision model for robot node " << robotColModelName;
// robotCollisionModel = robot->getRobotNode(robotColModelName)->getCollisionModel()->clone();
+ ARMARX_CHECK_NOT_NULL(robotCollisionModel);
robotCollisionModel->setGlobalPose(robot->getGlobalPose());
// static auto cylinder = VirtualRobot::ObjectIO::loadObstacle("/home/fabi/cylinder.xml");
@@ -194,7 +195,6 @@ namespace armarx::navigation::algorithm::astar
// robotCollisionModel = cylinder->getCollisionModel();
- ARMARX_CHECK_NOT_NULL(robotCollisionModel);
createUniformGrid();
ARMARX_DEBUG << "Setting start node for position " << start;
diff --git a/source/armarx/navigation/client/ComponentPlugin.cpp b/source/armarx/navigation/client/ComponentPlugin.cpp
index 3578b69541b521fad45360ffa78e794a5c7dc180..6266c680f8fb74e54b51735bb07310fb09b1a94c 100644
--- a/source/armarx/navigation/client/ComponentPlugin.cpp
+++ b/source/armarx/navigation/client/ComponentPlugin.cpp
@@ -1,10 +1,19 @@
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <memory>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+
#include <armarx/navigation/client/ComponentPlugin.h>
#include <armarx/navigation/client/Navigator.h>
+#include <armarx/navigation/client/services/MemorySubscriber.h>
+#include <armarx/navigation/client/services/SimpleEventHandler.h>
// ComponentPlugin
@@ -36,16 +45,62 @@ armarx::navigation::client::ComponentPlugin::preOnConnectComponent()
ARMARX_TRACE;
const std::string componentName = parent().getName();
- IceNavigator& srv = parent<armarx::navigation::client::ComponentPluginUser>().iceNavigator;
ARMARX_CHECK_NOT_NULL(navigatorPrx) << "Navigator proxy is null!";
- srv.setNavigatorComponent(navigatorPrx);
+ iceNavigator.setNavigatorComponent(navigatorPrx);
+}
+
+void
+armarx::navigation::client::ComponentPlugin::configureNavigator(
+ const client::NavigationStackConfig& stackConfig,
+ const std::string& configId)
+{
+ ARMARX_TRACE;
+
+ ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
+
+ eventHandler = [&]() -> std::unique_ptr<SimpleEventHandler>
+ {
+ if (parentDerives<armarx::armem::client::plugins::ListeningPluginUser>())
+ {
+ ARMARX_INFO << "Using memory event callbacks.";
+ auto memoryNameSystem =
+ parent<armarx::armem::client::plugins::ListeningPluginUser>().memoryNameSystem();
+ return std::make_unique<MemorySubscriber>(configId, memoryNameSystem);
+ }
+
+ ARMARX_INFO << "Cannot use memory event callbacks as this component is not derived from "
+ "`armarx::armem::client::plugins::ListeningPluginUser`";
+
+ return std::make_unique<SimpleEventHandler>();
+ }();
+
+ iceNavigator.createConfig(stackConfig, configId);
+
+ navigator = std::make_unique<Navigator>(
+ Navigator::InjectedServices{.navigator = &iceNavigator, .subscriber = eventHandler.get()});
+}
+void
+armarx::navigation::client::ComponentPlugin::configureNavigator(
+ const armarx::navigation::client::NavigationStackConfig& stackConfig,
+ const std::string& configId,
+ armarx::armem::client::MemoryNameSystem& memoryNameSystem)
+{
+ ARMARX_TRACE;
+
+ ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
+
+ eventHandler = std::make_unique<MemorySubscriber>(configId, memoryNameSystem);
+
+ iceNavigator.createConfig(stackConfig, configId);
+
+ navigator = std::make_unique<Navigator>(
+ Navigator::InjectedServices{.navigator = &iceNavigator, .subscriber = eventHandler.get()});
}
// ComponentPluginUser
-armarx::navigation::client::ComponentPluginUser::ComponentPluginUser() :
- navigator{navigatorServices}
+armarx::navigation::client::ComponentPluginUser::ComponentPluginUser()
{
ARMARX_TRACE;
addPlugin(plugin);
@@ -55,15 +110,16 @@ void
armarx::navigation::client::ComponentPluginUser::configureNavigator(
const client::NavigationStackConfig& stackConfig)
{
- configureNavigator(stackConfig, getName());
+ ARMARX_TRACE;
+ plugin->configureNavigator(stackConfig, getName());
}
-void
-armarx::navigation::client::ComponentPluginUser::configureNavigator(
- const client::NavigationStackConfig& stackConfig,
- const std::string& configId)
+armarx::navigation::client::Navigator&
+armarx::navigation::client::ComponentPluginUser::getNavigator()
{
- iceNavigator.createConfig(stackConfig, configId);
+ ARMARX_CHECK_NOT_NULL(plugin->navigator)
+ << "You need to call `configureNavigator()` before you can access the navigator!";
+ return *plugin->navigator;
}
armarx::navigation::client::ComponentPluginUser::~ComponentPluginUser() = default;
diff --git a/source/armarx/navigation/client/ComponentPlugin.h b/source/armarx/navigation/client/ComponentPlugin.h
index f64ce55b7a22545d49dcd99e2d992c589d6449c8..38ab565fb95c1bea71ee78e4838dbb6f050771ed 100644
--- a/source/armarx/navigation/client/ComponentPlugin.h
+++ b/source/armarx/navigation/client/ComponentPlugin.h
@@ -4,21 +4,27 @@
#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/core/ManagedIceObject.h>
+#include <armarx/navigation/client/ComponentPlugin.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/client/services/IceNavigator.h>
-#include <armarx/navigation/client/services/MemorySubscriber.h>
#include <armarx/navigation/client/services/SimpleEventHandler.h>
+namespace armarx::armem::client
+{
+ class MemoryNameSystem;
+}
namespace armarx::navigation::client
{
- class ComponentPlugin : virtual public armarx::ComponentPlugin
- {
+ class Navigator;
+ class ComponentPlugin : public armarx::ComponentPlugin
+ {
public:
- using armarx::ComponentPlugin::ComponentPlugin;
+ ComponentPlugin(ManagedIceObject& parent, const std::string& prefix) :
+ armarx::ComponentPlugin(parent, prefix){};
~ComponentPlugin() override;
@@ -28,10 +34,22 @@ namespace armarx::navigation::client
void preOnConnectComponent() override;
+ void configureNavigator(const client::NavigationStackConfig& stackConfig,
+ const std::string& configId);
+
+ void configureNavigator(const client::NavigationStackConfig& stackConfig,
+ const std::string& configId, armarx::armem::client::MemoryNameSystem& memoryNameSystem);
+
private:
static constexpr const char* PROPERTY_NAME = "nav.NavigatorName";
client::NavigatorInterfacePrx navigatorPrx;
+ IceNavigator iceNavigator;
+
+ std::unique_ptr<SimpleEventHandler> eventHandler;
+
+ public:
+ std::unique_ptr<Navigator> navigator;
};
@@ -43,30 +61,14 @@ namespace armarx::navigation::client
void configureNavigator(const client::NavigationStackConfig& stackConfig);
- void configureNavigator(const client::NavigationStackConfig& stackConfig,
- const std::string& configId);
-
-
+ Navigator& getNavigator();
+
// Non-API
-
- public:
~ComponentPluginUser() override;
- friend class ComponentPlugin;
-
private:
- IceNavigator iceNavigator;
- // MemorySubscriber memorySubscriber;
- SimpleEventHandler memorySubscriber; // FIXME
- Navigator::InjectedServices navigatorServices{.navigator = &iceNavigator,
- .subscriber = &memorySubscriber};
-
-
ComponentPlugin* plugin = nullptr;
-
- public:
- Navigator navigator;
};
} // namespace armarx::navigation::client
diff --git a/source/armarx/navigation/client/Navigator.cpp b/source/armarx/navigation/client/Navigator.cpp
index 8e8ac4ce358d4ca405dd2e68f771415a8b33be79..9780c8c009164d4a5ea1753ac9089a48f632ecab 100644
--- a/source/armarx/navigation/client/Navigator.cpp
+++ b/source/armarx/navigation/client/Navigator.cpp
@@ -3,6 +3,7 @@
#include <algorithm>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/client/ice_conversions.h>
@@ -12,6 +13,7 @@ namespace armarx::navigation::client
Navigator::Navigator(const InjectedServices& services) : srv{services}
{
+ ARMARX_TRACE;
// these checks cannot be used when component is started in property-readout mode
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
ARMARX_CHECK_NOT_NULL(srv.subscriber) << "Subscriber service must not be null!";
@@ -28,6 +30,7 @@ namespace armarx::navigation::client
void
Navigator::moveTo(const core::Pose& pose, core::NavigationFrame frame)
{
+ ARMARX_TRACE;
moveTo(std::vector<core::Pose>{pose}, frame);
}
@@ -35,6 +38,7 @@ namespace armarx::navigation::client
void
Navigator::moveTo(const std::vector<core::Pose>& waypoints, core::NavigationFrame frame)
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
srv.navigator->moveTo(waypoints, frame);
}
@@ -43,6 +47,7 @@ namespace armarx::navigation::client
void
Navigator::moveTo(const PathBuilder& builder, core::NavigationFrame frame)
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
const std::vector<WaypointTarget>& path = builder.path();
@@ -55,6 +60,7 @@ namespace armarx::navigation::client
void
Navigator::moveTowards(const core::Direction& direction, core::NavigationFrame frame)
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
srv.navigator->moveTowards(direction, frame);
}
@@ -63,6 +69,7 @@ namespace armarx::navigation::client
void
Navigator::pause()
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
srv.navigator->pause();
}
@@ -71,6 +78,7 @@ namespace armarx::navigation::client
void
Navigator::resume()
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
srv.navigator->resume();
}
@@ -79,6 +87,7 @@ namespace armarx::navigation::client
void
Navigator::stop()
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
srv.navigator->stop();
}
@@ -87,6 +96,7 @@ namespace armarx::navigation::client
void
Navigator::onGoalReached(const std::function<void(void)>& callback)
{
+ ARMARX_TRACE;
onGoalReached([&callback](const core::GoalReachedEvent&) { callback(); });
}
@@ -106,6 +116,7 @@ namespace armarx::navigation::client
StopEvent
Navigator::waitForStop()
{
+ ARMARX_TRACE;
std::unique_lock l{stoppedInfo.m};
stoppedInfo.cv.wait(l, [&i = stoppedInfo] { return i.event.has_value(); });
@@ -118,6 +129,7 @@ namespace armarx::navigation::client
void
Navigator::stopped(const StopEvent& e)
{
+ ARMARX_TRACE;
{
std::scoped_lock l{stoppedInfo.m};
stoppedInfo.event = e;
diff --git a/source/armarx/navigation/client/services/IceNavigator.cpp b/source/armarx/navigation/client/services/IceNavigator.cpp
index 6df5101d2623fb475878bc137341601e1ac583ec..a28e70467d5f3932638d1b90e248dcd36051ad89 100644
--- a/source/armarx/navigation/client/services/IceNavigator.cpp
+++ b/source/armarx/navigation/client/services/IceNavigator.cpp
@@ -43,6 +43,7 @@ namespace armarx::navigation::client
IceNavigator::createConfig(const client::NavigationStackConfig& config,
const std::string& configId)
{
+ ARMARX_TRACE;
this->configId = configId;
navigator->createConfig(config.toAron(), configId);
}
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 95f9410c394472dfd196b8dd692c99ad41756ca1..6532665f5ec93209e1066526118f68142ec3a830 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -322,7 +322,7 @@ namespace armarx::navigation::components
const Ice::Current&)
{
// TODO: Error handling.
-
+ ARMARX_TRACE;
ARMARX_INFO << "MoveTowards requested by caller '" << callerId << "'";
ARMARX_CHECK(navigators.count(callerId) > 0)
@@ -344,6 +344,7 @@ namespace armarx::navigation::components
void
components::Navigator::resume(const std::string& configId, const Ice::Current&)
{
+ ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
<< "Navigator config for caller `" << configId << "` not registered!";
navigators.at(configId).resume();
@@ -352,6 +353,7 @@ namespace armarx::navigation::components
void
components::Navigator::stop(const std::string& configId, const Ice::Current&)
{
+ ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
<< "Navigator config for caller `" << configId << "` not registered!";
navigators.at(configId).stop();
@@ -360,6 +362,7 @@ namespace armarx::navigation::components
void
components::Navigator::stopAll(const Ice::Current&)
{
+ ARMARX_TRACE;
for (auto& [_, navigator] : navigators)
{
navigator.stop();
@@ -369,6 +372,7 @@ namespace armarx::navigation::components
bool
components::Navigator::isPaused(const std::string& configId, const Ice::Current&)
{
+ ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
<< "Navigator config for caller `" << configId << "` not registered!";
return navigators.at(configId).isPaused();
@@ -377,6 +381,7 @@ namespace armarx::navigation::components
bool
components::Navigator::isStopped(const std::string& configId, const Ice::Current&)
{
+ ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
<< "Navigator config for caller `" << configId << "` not registered!";
return navigators.at(configId).isStopped();
@@ -511,6 +516,7 @@ namespace armarx::navigation::components
void
components::Navigator::updateRobot()
{
+ ARMARX_TRACE;
synchronizeLocalClone(scene.robot);
auto robotDescription = virtualRobotReader.queryDescription("Armar6", armem::Time::now());
@@ -540,6 +546,7 @@ namespace armarx::navigation::components
server::Navigator*
components::Navigator::activeNavigator()
{
+ ARMARX_TRACE;
// We define the active navigator to be the one whose movement is enabled.
const auto isActive = [](auto& nameNavPair) -> bool
{
diff --git a/source/armarx/navigation/components/example_client/Component.cpp b/source/armarx/navigation/components/example_client/Component.cpp
index 1a8b1fdda8526184bd7052d3b0a061556ad179d6..5b702a537dfc0b489e0ef23bf363a923e1b0bceb 100644
--- a/source/armarx/navigation/components/example_client/Component.cpp
+++ b/source/armarx/navigation/components/example_client/Component.cpp
@@ -103,10 +103,10 @@ namespace armarx::navigation::components::example_client
.trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
// Example of registering a lambda as callback.
- navigator.onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
+ getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
// Example of registering a method as callback.
- navigator.onGoalReached([this] { goalReached(); });
+ getNavigator().onGoalReached([this] { goalReached(); });
std::this_thread::sleep_for(1s);
@@ -115,12 +115,12 @@ namespace armarx::navigation::components::example_client
core::Pose goal = core::Pose::Identity();
goal.translation() << 2000, 1000, 0;
- navigator.moveTo(goal, core::NavigationFrame::Absolute);
+ getNavigator().moveTo(goal, core::NavigationFrame::Absolute);
std::this_thread::sleep_for(15s);
// Wait until goal is reached
- armarx::navigation::client::StopEvent se = navigator.waitForStop();
+ armarx::navigation::client::StopEvent se = getNavigator().waitForStop();
if (se)
{
ARMARX_INFO << "Goal 1 reached.";
@@ -143,12 +143,12 @@ namespace armarx::navigation::components::example_client
}
goal.translation() << -1500, 1000, 0;
- navigator.moveTo(goal, core::NavigationFrame::Absolute);
+ getNavigator().moveTo(goal, core::NavigationFrame::Absolute);
std::this_thread::sleep_for(15s);
// Wait until goal is reached
- se = navigator.waitForStop();
+ se = getNavigator().waitForStop();
if (se)
{
ARMARX_INFO << "Goal 2 reached.";
@@ -159,10 +159,10 @@ namespace armarx::navigation::components::example_client
}
goal.translation() << 4500, 4500, 0;
- navigator.moveTo(goal, core::NavigationFrame::Absolute);
+ getNavigator().moveTo(goal, core::NavigationFrame::Absolute);
// Wait until goal is reached
- se = navigator.waitForStop();
+ se = getNavigator().waitForStop();
if (se)
{
ARMARX_INFO << "Goal 3 reached.";
@@ -178,12 +178,12 @@ namespace armarx::navigation::components::example_client
ARMARX_INFO << "Moving into certain direction.";
// Start moving towards a direction
- navigator.moveTowards(core::Direction(100, 100, 0), core::NavigationFrame::Relative);
+ getNavigator().moveTowards(core::Direction(100, 100, 0), core::NavigationFrame::Relative);
std::this_thread::sleep_for(3s);
ARMARX_INFO << "Pausing movement.";
- navigator.pause();
+ getNavigator().pause();
}
void
@@ -206,10 +206,10 @@ namespace armarx::navigation::components::example_client
.trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
// Example of registering a lambda as callback.
- navigator.onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
+ getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
// Example of registering a method as callback.
- navigator.onGoalReached([this] { goalReached(); });
+ getNavigator().onGoalReached([this] { goalReached(); });
std::this_thread::sleep_for(1s);
@@ -232,12 +232,12 @@ namespace armarx::navigation::components::example_client
// clang-format on
ARMARX_INFO << "Starting execution";
- navigator.moveTo(pathBuilder, core::NavigationFrame::Absolute);
+ getNavigator().moveTo(pathBuilder, core::NavigationFrame::Absolute);
std::this_thread::sleep_for(25s); // TODO(fabian.reister): remove in the future
// Wait until goal is reached
- armarx::navigation::client::StopEvent se = navigator.waitForStop();
+ armarx::navigation::client::StopEvent se = getNavigator().waitForStop();
if (se)
{
ARMARX_INFO << "Goal 1 reached.";
@@ -260,7 +260,7 @@ namespace armarx::navigation::components::example_client
}
ARMARX_INFO << "Pausing movement.";
- navigator.pause();
+ getNavigator().pause();
}
diff --git a/source/armarx/navigation/core/Graph.cpp b/source/armarx/navigation/core/Graph.cpp
index f1bc378109fa48f6ed104e728467866474426dec..ac2fc21e9c497d0bff55e7baf354042625afcee4 100644
--- a/source/armarx/navigation/core/Graph.cpp
+++ b/source/armarx/navigation/core/Graph.cpp
@@ -81,7 +81,8 @@ namespace armarx::navigation::core
ss << " cost " << edge.attrib().cost()
<< ", type: " << static_cast<int>(edge.attrib().strategy) << " , has traj "
- << edge.attrib().trajectory.has_value() << ", aron: " << nlohmann::json(edge.attrib().aron);
+ << edge.attrib().trajectory.has_value();
+ // << ", aron: " << nlohmann::json(edge.attrib().aron);
return ss.str();
}
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index 8cc0acb31627e0a5ac566a528f5394358bab32e1..9d6f0033ad042ea2c447632179b13c78bfc865ac 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -6,6 +6,7 @@
#include <Eigen/Geometry>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
@@ -73,58 +74,60 @@ namespace armarx::navigation::glob_plan
{
}
- auto
- robotCollisionModel()
- {
+ // auto
+ // robotCollisionModel()
+ // {
- constexpr float platformHeight = 600;
- constexpr float bodyHeight = 1400;
- constexpr float fullHeight = platformHeight + bodyHeight;
+ // constexpr float platformHeight = 600;
+ // constexpr float bodyHeight = 1400;
+ // constexpr float fullHeight = platformHeight + bodyHeight;
- constexpr float platformRadius = 500.F;
- constexpr float bodyRadius = 300.F;
- constexpr float simpleRadius = std::max(platformRadius, bodyRadius);
+ // constexpr float platformRadius = 600.F;
+ // constexpr float bodyRadius = 300.F;
+ // constexpr float simpleRadius = std::max(platformRadius, bodyRadius);
- // combined model made out of two stacked cylinders.
- [[maybe_unused]]
- const auto createCombinedVisuModel = []()
- {
- VirtualRobot::CoinVisualizationFactory factory;
+ // // combined model made out of two stacked cylinders.
+ // [[maybe_unused]] const auto createCombinedVisuModel = []()
+ // {
+ // VirtualRobot::CoinVisualizationFactory factory;
- auto cylinderPlatform = factory.createCylinder(platformRadius, platformHeight);
- factory.applyDisplacement(
- cylinderPlatform,
- core::Pose(Eigen::Translation3f{Eigen::Vector3f{0, 0, platformHeight / 2}})
- .matrix());
+ // auto cylinderPlatform = factory.createCylinder(platformRadius, platformHeight);
+ // factory.applyDisplacement(
+ // cylinderPlatform,
+ // core::Pose(Eigen::Translation3f{platformHeight / 2 * Eigen::Vector3f::UnitY()})
+ // .matrix());
- auto cylinderBody = factory.createCylinder(bodyRadius, bodyHeight);
- factory.applyDisplacement(cylinderPlatform,
- core::Pose(Eigen::Translation3f{Eigen::Vector3f{
- 0, 0, bodyHeight / 2 + platformHeight}})
- .matrix());
+ // auto cylinderBody = factory.createCylinder(bodyRadius, bodyHeight);
+ // factory.applyDisplacement(
+ // cylinderPlatform,
+ // core::Pose(Eigen::Translation3f{(bodyHeight / 2 + platformHeight) *
+ // Eigen::Vector3f::UnitY()})
+ // .matrix());
- return factory.createUnitedVisualization({cylinderPlatform, cylinderBody});
- };
- // simple model made of one cylinder only
- const auto createSimpleVisuModel = []()
- {
- VirtualRobot::CoinVisualizationFactory factory;
+ // return factory.createUnitedVisualization({cylinderPlatform, cylinderBody});
+ // };
- auto cylinderPlatform = factory.createCylinder(simpleRadius, fullHeight);
- factory.applyDisplacement(
- cylinderPlatform,
- core::Pose(Eigen::Translation3f{Eigen::Vector3f{0, 0, fullHeight / 2}}).matrix());
+ // // simple model made of one cylinder only
+ // const auto createSimpleVisuModel = []()
+ // {
+ // VirtualRobot::CoinVisualizationFactory factory;
- return cylinderPlatform;
- };
+ // auto cylinderPlatform = factory.createCylinder(simpleRadius, fullHeight);
+ // factory.applyDisplacement(
+ // cylinderPlatform,
+ // core::Pose(Eigen::Translation3f{fullHeight / 2 * Eigen::Vector3f::UnitY()})
+ // .matrix());
- const auto visuModel = createSimpleVisuModel();
+ // return cylinderPlatform;
+ // };
- VirtualRobot::CollisionModelPtr collisionModel(new VirtualRobot::CollisionModel(visuModel));
+ // const auto visuModel = createSimpleVisuModel();
- return collisionModel;
- }
+ // VirtualRobot::CollisionModelPtr collisionModel(new VirtualRobot::CollisionModel(visuModel));
+
+ // return collisionModel;
+ // }
std::vector<Eigen::Vector2f>
AStar::postProcessPath(const std::vector<Eigen::Vector2f>& path)
@@ -160,7 +163,8 @@ namespace armarx::navigation::glob_plan
// planner.setRobotColModel("Platform-colmodel");
// planner.setRobotColModel("Platform-colmodel");
- planner.setRobotCollisionModel(robotCollisionModel());
+ //planner.setRobotCollisionModel(robotCollisionModel());
+ planner.setRobotCollisionModel(scene.robot->getRobotNode("Platform-navigation-colmodel")->getCollisionModel());
const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
diff --git a/source/armarx/navigation/global_planning/CMakeLists.txt b/source/armarx/navigation/global_planning/CMakeLists.txt
index 87d9bd68ab130f67d2dda3b4bbc027bde922dd50..4de838c4800f93ee893ef7ec22398800902063a6 100644
--- a/source/armarx/navigation/global_planning/CMakeLists.txt
+++ b/source/armarx/navigation/global_planning/CMakeLists.txt
@@ -30,3 +30,11 @@ armarx_add_library(global_planning
./aron_conversions.h
./optimization/OrientationOptimizer.h
)
+
+
+armarx_add_test(orientation_optimization_test
+ TEST_FILES
+ test/orientation_optimization_test.cpp
+ DEPENDENCIES
+ armarx_navigation::global_planning
+)
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
index 8cc3ec811b9a06e00e0c49e5c34704d683f9b311..eb6aeee7fd233d3eaa3f40a1dee42c2625273bb2 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
@@ -1,5 +1,7 @@
#include "OrientationOptimizer.h"
+#include <cmath>
+
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/drop.hpp>
#include <range/v3/view/drop_last.hpp>
@@ -8,9 +10,11 @@
#include <ceres/ceres.h>
#include <ceres/cost_function.h>
+#include <ceres/jet.h>
#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+#include <SimoxUtility/math/periodic/periodic_diff.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -29,16 +33,24 @@ namespace armarx::navigation::glob_plan
// problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x1);
- struct CostFunctor
+ inline auto
+ periodicDiff(auto a, auto b)
{
- template <typename T>
- bool
- operator()(const T* const x, T* residual) const
- {
- residual[0] = 10.0 - x[0];
- return true;
- }
- };
+ // angle to corresponding vector
+ const auto a1 = ceres::cos(a);
+ const auto a2 = ceres::sin(a);
+
+ // angle to corresponding vector
+ const auto b1 = ceres::cos(b);
+ const auto b2 = ceres::sin(b);
+
+ // cosine similarity
+ const auto cosSim = a1 * b1 + a2 * b2;
+
+ const auto angleDiff = ceres::acos(cosSim);
+
+ return angleDiff;
+ }
struct SmoothOrientationCostFunctor
{
@@ -54,9 +66,8 @@ namespace armarx::navigation::glob_plan
const T* const yawNext,
T* residual) const
{
- // TODO periodic
- residual[0] = weight * (yawPrev[0] - yaw[0]);
- residual[1] = weight * (yawNext[0] - yaw[0]);
+ residual[0] = weight * periodicDiff(yawPrev[0], yaw[0]);
+ residual[1] = weight * periodicDiff(yawNext[0], yaw[0]);
return true;
}
@@ -76,9 +87,8 @@ namespace armarx::navigation::glob_plan
bool
operator()(const T* const yaw, const T* const yawNext, T* residual) const
{
- // TODO periodic
- residual[0] = weight * (yawPre - yaw[0]);
- residual[1] = weight * (yawNext[0] - yaw[0]);
+ residual[0] = weight * periodicDiff(yawPre, yaw[0]);
+ residual[1] = weight * periodicDiff(yawNext[0], yaw[0]);
return true;
}
@@ -99,9 +109,8 @@ namespace armarx::navigation::glob_plan
bool
operator()(const T* const yawPre, const T* const yaw, T* residual) const
{
- // TODO periodic
- residual[0] = weight * (yawPre[0] - yaw[0]);
- residual[1] = weight * (yawNext - yaw[0]);
+ residual[0] = weight * periodicDiff(yawPre[0], yaw[0]);
+ residual[1] = weight * periodicDiff(yawNext, yaw[0]);
return true;
}
@@ -122,8 +131,7 @@ namespace armarx::navigation::glob_plan
bool
operator()(const T* const x, T* residual) const
{
- // TODO periodic
- residual[0] = weight * (prior - x[0]);
+ residual[0] = weight * periodicDiff(prior, x[0]);
return true;
}
@@ -163,31 +171,43 @@ namespace armarx::navigation::glob_plan
// ceres::AngleLocalParameterization::Create();
// prior for all waypoints
- for (size_t i = 1; i < (orientations.size() - 1); i++)
+ if (params.priorWeight > 0.F)
{
- ceres::CostFunction* priorCostFunction =
- new ceres::AutoDiffCostFunction<OrientationPriorCostFunctor, 1, 1>(
- new OrientationPriorCostFunctor(orientations.at(i), params.priorWeight));
-
- problem.AddResidualBlock(priorCostFunction, nullptr, &orientations.at(i));
+ for (size_t i = 1; i < (orientations.size() - 1); i++)
+ {
+ ceres::CostFunction* priorCostFunction =
+ new ceres::AutoDiffCostFunction<OrientationPriorCostFunctor, 1, 1>(
+ new OrientationPriorCostFunctor(orientations.at(i), params.priorWeight));
+
+ problem.AddResidualBlock(priorCostFunction, nullptr, &orientations.at(i));
+ }
}
+
// smooth waypoint orientation, no start and goal nodes involved!
- for (size_t i = 2; i < (orientations.size() - 2); i++)
+ if (params.smoothnessWeight > 0.F)
{
- ceres::CostFunction* smoothCostFunction =
- new ceres::AutoDiffCostFunction<SmoothOrientationCostFunctor, 2, 1, 1, 1>(
- new SmoothOrientationCostFunctor(params.smoothnessWeight));
-
- problem.AddResidualBlock(smoothCostFunction,
- nullptr,
- &orientations.at(i - 1),
- &orientations.at(i),
- &orientations.at(i + 1));
+ ARMARX_VERBOSE << "Enabled SmoothOrientationCost";
+ for (size_t i = 2; i < (orientations.size() - 2); i++)
+ {
+ ceres::CostFunction* smoothCostFunction =
+ new ceres::AutoDiffCostFunction<SmoothOrientationCostFunctor, 2, 1, 1, 1>(
+ new SmoothOrientationCostFunctor(params.smoothnessWeight));
+
+ problem.AddResidualBlock(smoothCostFunction,
+ nullptr,
+ &orientations.at(i - 1),
+ &orientations.at(i),
+ &orientations.at(i + 1));
+ }
}
+
// smooth waypoint orientation, involving start
+ if (params.smoothnessWeightStartGoal > 0.F)
{
+ ARMARX_VERBOSE << "Enabled SmoothOrientationFixedPreCost";
+
ceres::CostFunction* smoothCostFunction =
new ceres::AutoDiffCostFunction<SmoothOrientationFixedPreCostFunctor, 2, 1, 1>(
new SmoothOrientationFixedPreCostFunctor(orientations.front(),
@@ -198,7 +218,10 @@ namespace armarx::navigation::glob_plan
}
// smooth waypoint orientation, involving goal
+ if (params.smoothnessWeightStartGoal > 0.F)
{
+ ARMARX_VERBOSE << "Enabled SmoothOrientationFixedNextCost";
+
ceres::CostFunction* smoothCostFunction =
new ceres::AutoDiffCostFunction<SmoothOrientationFixedNextCostFunctor, 2, 1, 1>(
new SmoothOrientationFixedNextCostFunctor(orientations.back(),
@@ -215,12 +238,23 @@ namespace armarx::navigation::glob_plan
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
+ options.max_num_iterations = 100;
+ options.function_tolerance = 0.01;
+
ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
- std::cout << summary.BriefReport() << "\n";
+ std::cout << summary.FullReport() << "\n";
+ // std::cout << summary.BriefReport() << "\n";
ARMARX_INFO << "orientations after: " << orientations;
+ ARMARX_INFO << "Optimization: " << summary.iterations.size() << " iterations";
+
+ if(not summary.IsSolutionUsable())
+ {
+ ARMARX_ERROR << "Orientation optimization failed!";
+ // TODO write to file
+ }
const auto applyOrientation = [](const auto& p) -> core::TrajectoryPoint
{
diff --git a/source/armarx/navigation/global_planning/test/global_planningTest.cpp b/source/armarx/navigation/global_planning/test/global_planningTest.cpp
deleted file mode 100644
index 2518dcfda0e01e336fae50adbaf3db98756010f4..0000000000000000000000000000000000000000
--- a/source/armarx/navigation/global_planning/test/global_planningTest.cpp
+++ /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/>.
- *
- * @package Navigation::ArmarXObjects::global_planning
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::global_planning
-
-#define ARMARX_BOOST_TEST
-
-#include "../global_planning.h"
-
-#include <iostream>
-
-#include <armarx/navigation/Test.h>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
- BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp b/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c584146cbd704f87e1a7317a27cd7f513298024b
--- /dev/null
+++ b/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp
@@ -0,0 +1,128 @@
+/**
+ * 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/>.
+ *
+ * @package Navigation::ArmarXObjects::global_planning
+ * @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
+ */
+
+#include <cstddef>
+
+#include <boost/test/tools/old/interface.hpp>
+
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::global_planning
+
+#define ARMARX_BOOST_TEST
+
+#include <cmath>
+#include <iostream>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "armarx/navigation/core/basic_types.h"
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/global_planning/optimization/OrientationOptimizer.h>
+
+BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoal)
+{
+ namespace nav = armarx::navigation;
+ namespace gp = armarx::navigation::glob_plan;
+
+ nav::core::Path path;
+ path.emplace_back(nav::core::Pose::Identity());
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 500});
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 1000});
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 1500});
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 2000} *
+ Eigen::AngleAxis(M_PI_2f32, Eigen::Vector3f::UnitZ()));
+
+ const auto trajectory = nav::core::Trajectory::FromPath(path, 100);
+
+ gp::OrientationOptimizer::Params params{};
+
+
+ gp::OrientationOptimizer optimizer(trajectory, params);
+
+ const gp::OrientationOptimizer::OptimizationResult result = optimizer.optimize();
+
+ // # of points should be unchanged
+ BOOST_REQUIRE_EQUAL(result.trajectory.poses().size(), trajectory.poses().size());
+
+ // avoid an underflow in the for loop below
+ BOOST_REQUIRE_GE(trajectory.poses().size(), 2);
+
+ // loop over intermediate points
+ for (std::size_t i = 0; i < (result.trajectory.poses().size() - 2); i++)
+ {
+ const auto rpy = simox::math::mat3f_to_rpy(result.trajectory.poses().at(i).linear());
+ const auto yaw = rpy.z();
+
+ // The orientation should be changed compared to the unoptimized version.
+ // As the last pose is rotated by 90°, the intermediate poses should at least be
+ // changed gentlty into this direction.
+ BOOST_CHECK_GT(yaw, 0.1);
+ }
+}
+
+
+BOOST_AUTO_TEST_CASE(testOrientationOptimizationCircular)
+{
+ namespace nav = armarx::navigation;
+ namespace gp = armarx::navigation::glob_plan;
+
+ nav::core::Path path;
+ path.emplace_back(nav::core::Pose::Identity());
+ for (int i = 0; i < 20; i++)
+ {
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 500 * i} *
+ Eigen::AngleAxis(M_PI_2f32 * i, Eigen::Vector3f::UnitZ()));
+ }
+
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 500 * 20});
+
+
+ const auto trajectory = nav::core::Trajectory::FromPath(path, 100);
+
+ // Here we don't care about the orientatation prior.
+ // The orientation of the intermediate points should converge to 0 (same direction as start and goal)
+ gp::OrientationOptimizer::Params params{.priorWeight = 0.F};
+
+
+ gp::OrientationOptimizer optimizer(trajectory, params);
+
+ const gp::OrientationOptimizer::OptimizationResult result = optimizer.optimize();
+
+ // # of points should be unchanged
+ BOOST_REQUIRE_EQUAL(result.trajectory.poses().size(), trajectory.poses().size());
+
+ // avoid an underflow in the for loop below
+ BOOST_REQUIRE_GE(trajectory.poses().size(), 2);
+
+ // loop over intermediate points
+ for (std::size_t i = 0; i < (result.trajectory.poses().size() - 2); i++)
+ {
+ const auto rpy = simox::math::mat3f_to_rpy(result.trajectory.poses().at(i).linear());
+ const auto yaw = rpy.z();
+
+ // The orientation should be changed compared to the unoptimized version.
+ // As the last pose is oriented the same way as the start, the intermediate poses should
+ // have the same orientation. => yaw ~= 0
+ BOOST_CHECK_LT(std::abs(yaw), 0.01);
+ }
+}
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index bb1a6296e68d705e9806c163b2a9a895c461811c..5653dd7cb84ad375b35287853205f9f1e2d0dcff 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -88,7 +88,7 @@ namespace armarx::navigation::server
case armarx::navigation::client::GlobalPlanningStrategy::Free:
{
ARMARX_VERBOSE << "Global planning from " << start.translation() << " to "
- << goal.translation();
+ << goal.translation();
ARMARX_CHECK_NOT_NULL(config.stack.globalPlanner);
try
@@ -97,7 +97,8 @@ namespace armarx::navigation::server
if (globalPlan.has_value())
{
edge.attrib().trajectory = globalPlan->trajectory;
- ARMARX_VERBOSE << "Free path length: " << globalPlan->trajectory.length();
+ ARMARX_VERBOSE << "Free path length: "
+ << globalPlan->trajectory.length();
edge.attrib().cost() = globalPlan->trajectory.length();
}
else
@@ -109,7 +110,7 @@ namespace armarx::navigation::server
catch (...)
{
ARMARX_VERBOSE << "Global planning failed due to exception "
- << GetHandledExceptionString();
+ << GetHandledExceptionString();
edge.attrib().cost() = std::numeric_limits<float>::max();
}
@@ -162,7 +163,7 @@ namespace armarx::navigation::server
const core::Graph::ConstVertex& goalVertex)
{
ARMARX_VERBOSE << "Graph consists of " << graph.numVertices() << " vertices and "
- << graph.numEdges() << " edges.";
+ << graph.numEdges() << " edges.";
std::vector<core::Graph::VertexDescriptor> predecessors(graph.numVertices());
std::vector<int> d(num_vertices(graph));
@@ -189,7 +190,7 @@ namespace armarx::navigation::server
for (const auto& edge : graph.edges())
{
ARMARX_VERBOSE << edge.sourceObjectID() << " -> " << edge.targetObjectID() << ": "
- << edge.attrib().m_value;
+ << edge.attrib().m_value;
}
std::vector<core::Graph::EdgeDescriptor> edgesToBeRemoved;
@@ -210,15 +211,15 @@ namespace armarx::navigation::server
for (const auto& edge : graph.edges())
{
ARMARX_VERBOSE << edge.sourceObjectID() << " -> " << edge.targetObjectID() << ": "
- << edge.attrib().m_value;
+ << edge.attrib().m_value;
ARMARX_VERBOSE << "Edge: " << edge << "cost: " << edge.attrib().cost()
- << ", type: " << static_cast<int>(edge.attrib().strategy) << " , has traj "
- << edge.attrib().trajectory.has_value();
+ << ", type: " << static_cast<int>(edge.attrib().strategy)
+ << " , has traj " << edge.attrib().trajectory.has_value();
}
ARMARX_VERBOSE << "Searching shortest path from vertex with id `"
- << graph.vertex(start).objectID() << "` to vertex `"
- << graph.vertex(goalVertex.descriptor()).objectID() << "`";
+ << graph.vertex(start).objectID() << "` to vertex `"
+ << graph.vertex(goalVertex.descriptor()).objectID() << "`";
boost::dijkstra_shortest_paths(graph, start, params);
@@ -243,8 +244,9 @@ namespace armarx::navigation::server
// find edge between parent and currentVertex
auto outEdges = graph.vertex(parent).outEdges();
ARMARX_VERBOSE << "Parent has " << std::distance(outEdges.begin(), outEdges.end())
- << " out edges";
- ARMARX_CHECK(std::distance(outEdges.begin(), outEdges.end()) > 0); // not empty
+ << " out edges";
+
+ ARMARX_CHECK_GREATER(std::distance(outEdges.begin(), outEdges.end()), 0) << "Cannot reach another vertex from vertex `" << graph.vertex(parent).objectID(); // not empty
auto edgeIt = std::find_if(outEdges.begin(),
outEdges.end(),
@@ -361,18 +363,25 @@ namespace armarx::navigation::server
// resample event straight lines
ARMARX_CHECK_NOT_EMPTY(trajectoryPoints);
- const core::Trajectory segmentTraj({trajectoryPoints.back(), nextTrajPt});
- ARMARX_INFO << "Segment trajectory length: " << segmentTraj.length();
+ // const core::Trajectory segmentTraj({trajectoryPoints.back(), nextTrajPt});
+ // ARMARX_INFO << "Segment trajectory length: " << segmentTraj.length();
+
+ // const auto resampledTrajectory = segmentTraj.resample(500); // FIXME param
- const auto resampledTrajectory = segmentTraj.resample(500); // FIXME param
+ // ARMARX_INFO << "Resampled trajectory contains "
+ // << resampledTrajectory.points().size() << " points";
- ARMARX_INFO << "Resampled trajectory contains "
- << resampledTrajectory.points().size() << " points";
+ // this is the same pose as the goal of the previous segment but with a different velocity
+ // FIXME MUST set velocity here.
+ // trajectoryPoints.push_back(
+ // core::TrajectoryPoint{.waypoint = trajectoryPoints.back().waypoint,
+ // .velocity = std::numeric_limits<float>::max()});
ARMARX_TRACE;
- trajectoryPoints.insert(trajectoryPoints.end(),
- resampledTrajectory.points().begin(),
- resampledTrajectory.points().end());
+ // trajectoryPoints.insert(trajectoryPoints.end(),
+ // resampledTrajectory.points().begin(),
+ // resampledTrajectory.points().end());
+ trajectoryPoints.push_back(nextTrajPt);
break;
}
@@ -533,6 +542,8 @@ namespace armarx::navigation::server
ARMARX_VERBOSE << graph;
+ srv.introspector->onGlobalGraph(graph);
+
ARMARX_TRACE;
const auto shortestPath = findShortestPath(graph, startVertex.value(), goalVertex);
@@ -858,8 +869,9 @@ namespace armarx::navigation::server
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;
@@ -868,7 +880,7 @@ namespace armarx::navigation::server
robotFrameVelocity.angular = twist.angular;
ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
- << robotFrameVelocity.linear;
+ << robotFrameVelocity.linear;
srv.executor->move(robotFrameVelocity);
}
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index 21e2638773cf1b36fb2b3684a48a2bf81ed090df..5607b29e86c11db028cce98f317cee24dbb1bfbf 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -175,4 +175,38 @@ namespace armarx::navigation::server
return *this;
}
+ void
+ ArvizIntrospector::onGlobalGraph(const core::Graph& graph)
+ {
+ auto layer = arviz.layer("global_planning_graph");
+
+
+ for (const auto& edge : graph.edges())
+ {
+
+ const auto sourcePose = graph.vertex(edge.sourceDescriptor()).attrib().getPose();
+ const auto targetPose = graph.vertex(edge.targetDescriptor()).attrib().getPose();
+
+
+ const viz::Color color = [&]()
+ {
+ if (edge.attrib().cost() > 100'000) // "NaN check"
+ {
+ return viz::Color::red();
+ }
+
+ return viz::Color::green();
+ }();
+
+
+ layer.add(viz::Arrow(std::to_string(edge.sourceObjectID().t) + " -> " +
+ std::to_string(edge.targetObjectID().t))
+ .fromTo(sourcePose.translation(), targetPose.translation())
+ .color(color));
+ }
+
+ arviz.commit(layer);
+ }
+
+
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index cc67dccd67355215c411f7cdf8a6d8df8b25b660..8d20cd8cccc608f04d10dcf76888821817e968d2 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -61,6 +61,9 @@ namespace armarx::navigation::server
void onGlobalShortestPath(const std::vector<core::Pose>& path) override;
+ void onGlobalGraph(const core::Graph& graph) override;
+
+
// // Non-API
ArvizIntrospector(ArvizIntrospector&& other) noexcept;
diff --git a/source/armarx/navigation/server/introspection/IntrospectorInterface.h b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
index 50b6ca851beeb9380cea632da87601450143e469..316cb9baa11690e2d1bfcc0a72cecb9ba8973264 100644
--- a/source/armarx/navigation/server/introspection/IntrospectorInterface.h
+++ b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
@@ -47,6 +47,8 @@ namespace armarx::navigation::server
virtual void onGlobalShortestPath(const std::vector<core::Pose>& path) = 0;
+ virtual void onGlobalGraph(const core::Graph& graph) = 0;
+
protected:
private:
};
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.h b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
index 0e4a53549ea40ce726fcc3d44721fc3648662296..2efb102a0e5467a6ae659be7e29c59319f702dc1 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.h
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
@@ -51,6 +51,11 @@ namespace armarx::navigation::server
{
}
+ void
+ onGlobalGraph(const core::Graph& graph) override
+ {
+ }
+
protected:
private: