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 a53c8528d01d3d06a8bf41461871dcd67accdb08..ed564421cab8702c61b66b7a8d49b84b76baffd1 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -314,7 +314,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)
@@ -334,6 +334,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();
@@ -342,6 +343,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();
@@ -350,6 +352,7 @@ namespace armarx::navigation::components
void
components::Navigator::stopAll(const Ice::Current&)
{
+ ARMARX_TRACE;
for (auto& [_, navigator] : navigators)
{
navigator.stop();
@@ -359,6 +362,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();
@@ -367,6 +371,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();
@@ -501,6 +506,7 @@ namespace armarx::navigation::components
void
components::Navigator::updateRobot()
{
+ ARMARX_TRACE;
synchronizeLocalClone(scene.robot);
auto robotDescription = virtualRobotReader.queryDescription("Armar6", armem::Time::now());
@@ -530,6 +536,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
{