diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index f0d215d5e04f7376eddbfca27952300cc585f14d..2be9420b49865c6bd982454f34944957f4de9a5d 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -18,12 +18,14 @@
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <VirtualRobot/Robot.h>
-#include <ArmarXCore/core/time/Clock.h>
+#include "ArmarXCore/core/time/StopWatch.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include "armarx/navigation/local_planning/LocalPlanner.h"
#include "range/v3/algorithm/reverse.hpp"
#include "range/v3/range/conversion.hpp"
#include <SemanticObjectRelations/Shapes/Shape.h>
@@ -149,11 +151,12 @@ namespace armarx::navigation::server
break;
case core::NavigationFrame::Relative:
globalWaypoints.reserve(waypoints.size());
- std::transform(std::begin(waypoints),
- std::end(waypoints),
- std::back_inserter(globalWaypoints),
- [&](const core::Pose& p)
- { return core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()) * p; });
+ std::transform(
+ std::begin(waypoints),
+ std::end(waypoints),
+ std::back_inserter(globalWaypoints),
+ [&](const core::Pose& p)
+ { return core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()) * p; });
break;
}
@@ -383,7 +386,7 @@ namespace armarx::navigation::server
.velocity = 0};
// resample event straight lines
-// ARMARX_CHECK_NOT_EMPTY(trajectoryPoints);
+ // ARMARX_CHECK_NOT_EMPTY(trajectoryPoints);
// const core::Trajectory segmentTraj({trajectoryPoints.back(), nextTrajPt});
// ARMARX_INFO << "Segment trajectory length: " << segmentTraj.length();
@@ -481,8 +484,8 @@ namespace armarx::navigation::server
// instead. Thus, we collect all routes to reach the node.
if (not target.locationId->empty())
{
- const auto& subgraph =
- core::getSubgraph(target.locationId.value(), srv.sceneProvider->scene().graph->subgraphs);
+ const auto& subgraph = core::getSubgraph(
+ target.locationId.value(), srv.sceneProvider->scene().graph->subgraphs);
const auto vertex = core::getVertexByName(target.locationId.value(), subgraph);
@@ -544,8 +547,8 @@ namespace armarx::navigation::server
ARMARX_TRACE;
goalReachedMonitor = std::nullopt;
- goalReachedMonitor =
- GoalReachedMonitor(graphBuilder.goalPose(), srv.sceneProvider->scene(), GoalReachedMonitorConfig());
+ goalReachedMonitor = GoalReachedMonitor(
+ graphBuilder.goalPose(), srv.sceneProvider->scene(), GoalReachedMonitorConfig());
if (goalReachedMonitor->goalReached())
{
@@ -553,9 +556,9 @@ namespace armarx::navigation::server
<< goalReachedMonitor->goal().translation().head<2>()
<< ". Robot won't move.";
- srv.publisher->goalReached(
- core::GoalReachedEvent{{armarx::Clock::Now()},
- core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
+ srv.publisher->goalReached(core::GoalReachedEvent{
+ {armarx::Clock::Now()},
+ core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
return;
}
@@ -602,12 +605,12 @@ namespace armarx::navigation::server
// the following is similar to moveToAbsolute
// TODO(fabian.reister): remove code duplication
- ARMARX_TRACE;
+ srv.executor->execute(globalPlan->trajectory);
+ ARMARX_TRACE;
srv.publisher->globalTrajectoryUpdated(globalPlan.value());
ARMARX_TRACE;
-
srv.introspector->onGlobalPlannerResult(globalPlan.value());
ARMARX_INFO << "Global planning completed. Will now start all required threads";
@@ -620,35 +623,21 @@ namespace armarx::navigation::server
Navigator::startStack()
{
- // local planner
- if (config.stack.localPlanner)
- {
- ARMARX_TRACE;
-
- ARMARX_INFO << "Planning local trajectory enabled";
- replanningTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::replan,
- config.general.tasks.replanningUpdatePeriod,
- false,
- "LocalPlannerTask");
- replanningTask->start();
- }
-
+ ARMARX_TRACE;
- // monitoring
- ARMARX_INFO << "Starting monitoring";
- monitorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateMonitor,
- config.general.tasks.monitorUpdatePeriod,
+ ARMARX_INFO << "Planning local trajectory enabled";
+ runningTask = new PeriodicTask<Navigator>(this,
+ &Navigator::run,
+ config.general.tasks.replanningUpdatePeriod,
false,
- "MonitorTask");
- monitorTask->start();
+ "LocalPlannerTask");
+ runningTask->start();
+
// Could be required if pauseMovement() has been called in the past.
resume();
srv.publisher->movementStarted(core::MovementStartedEvent{
- {armarx::Clock::Now()},core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
+ {armarx::Clock::Now()}, core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
}
void
@@ -666,21 +655,21 @@ namespace armarx::navigation::server
ARMARX_TRACE;
ARMARX_CHECK_NOT_EMPTY(waypoints);
- ARMARX_INFO << "Request to move from " << srv.sceneProvider->scene().robot->getGlobalPose() << " to "
- << waypoints.back().matrix();
+ ARMARX_INFO << "Request to move from " << srv.sceneProvider->scene().robot->getGlobalPose()
+ << " to " << waypoints.back().matrix();
// first we check if we are already at the goal position
goalReachedMonitor = std::nullopt;
- goalReachedMonitor =
- GoalReachedMonitor(waypoints.back(), srv.sceneProvider->scene(), GoalReachedMonitorConfig());
+ goalReachedMonitor = GoalReachedMonitor(
+ waypoints.back(), srv.sceneProvider->scene(), GoalReachedMonitorConfig());
if (goalReachedMonitor->goalReached())
{
ARMARX_INFO << "Already at goal position. Robot won't move.";
- srv.publisher->goalReached(
- core::GoalReachedEvent{{armarx::Clock::Now()},
- core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
+ srv.publisher->goalReached(core::GoalReachedEvent{
+ {armarx::Clock::Now()},
+ core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
return;
}
@@ -699,51 +688,22 @@ namespace armarx::navigation::server
if (not globalPlan.has_value())
{
ARMARX_WARNING << "No global trajectory. Cannot move.";
- srv.publisher->globalPlanningFailed(core::GlobalPlanningFailedEvent{
- {.timestamp = armarx::Clock::Now()}, {""}});
+ srv.publisher->globalPlanningFailed(
+ core::GlobalPlanningFailedEvent{{.timestamp = armarx::Clock::Now()}, {""}});
srv.introspector->failure();
return;
}
- ARMARX_TRACE;
+ ARMARX_TRACE;
srv.publisher->globalTrajectoryUpdated(globalPlan.value());
srv.introspector->onGlobalPlannerResult(globalPlan.value());
+ srv.executor->execute(globalPlan->trajectory);
ARMARX_INFO << "Global planning completed. Will now start all required threads";
ARMARX_TRACE;
- // local planner
- if (config.stack.localPlanner)
- {
- ARMARX_INFO << "Planning local trajectory enabled";
- replanningTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::replan,
- config.general.tasks.replanningUpdatePeriod,
- false,
- "LocalPlannerTask");
- replanningTask->start();
- }
-
- // monitoring
- ARMARX_INFO << "Starting monitoring";
- monitorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateMonitor,
- config.general.tasks.monitorUpdatePeriod,
- false,
- "MonitorTask");
- monitorTask->start();
-
- // Could be required if pauseMovement() has been called in the past.
- ARMARX_INFO << "Resuming ...";
- ARMARX_TRACE;
- resume();
- ARMARX_INFO << "Done.";
-
- // TODO check if startPose shouldn't be waypoints.front() etc
- srv.publisher->movementStarted(core::MovementStartedEvent{
- {armarx::Clock::Now()}, {waypoints.back()}});
+ startStack();
ARMARX_INFO << "Movement started.";
}
@@ -757,26 +717,60 @@ namespace armarx::navigation::server
Navigator::moveTowardsAbsolute(const core::Direction& direction)
{
}
-
+
void
- Navigator::replan()
+ Navigator::run()
{
- updateScene();
- updateLocalPlanner();
- updateExecutor();
- updateIntrospector();
+ // TODO(fabian.reister): add debug observer logging
+
+ // scene update
+ {
+ const Duration duration =
+ armarx::core::time::StopWatch::measure([&]() { updateScene(); });
+
+ ARMARX_VERBOSE << deactivateSpam(0.2) << "Scene update: " << duration.toSecondsDouble()
+ << "s.";
+ }
+
+ // local planner update
+ {
+ const Duration duration = armarx::core::time::StopWatch::measure(
+ [&]()
+ {
+ if (hasLocalPlanner())
+ {
+ const auto localPlannerResult = updateLocalPlanner();
+ updateExecutor(localPlannerResult);
+ updateIntrospector(localPlannerResult);
+ }
+ });
+ ARMARX_VERBOSE << deactivateSpam(0.2)
+ << "Local planner update: " << duration.toSecondsDouble() << "s.";
+ }
+
+ // monitor update
+ {
+ const Duration duration =
+ armarx::core::time::StopWatch::measure([&]() { updateMonitor(); });
+
+ ARMARX_VERBOSE << deactivateSpam(0.2)
+ << "Monitor update: " << duration.toSecondsDouble() << "s.";
+ }
}
-
- void Navigator::updateScene()
+
+ void
+ Navigator::updateScene()
{
ARMARX_CHECK_NOT_NULL(srv.sceneProvider);
srv.sceneProvider->synchronize(armarx::Clock::Now());
}
- void
+ std::optional<loc_plan::LocalPlannerResult>
Navigator::updateLocalPlanner()
{
+ ARMARX_CHECK(hasLocalPlanner());
+
localPlan = config.stack.localPlanner->plan(globalPlan->trajectory);
if (localPlan.has_value())
@@ -787,10 +781,12 @@ namespace armarx::navigation::server
{
// srv.publisher->localTrajectoryPlanningFailed();
}
+
+ return localPlan;
}
void
- Navigator::updateExecutor()
+ Navigator::updateExecutor(const std::optional<loc_plan::LocalPlannerResult>& localPlan)
{
if (isPaused() or isStopped())
{
@@ -799,11 +795,10 @@ namespace armarx::navigation::server
return;
}
- if (not isStackResultValid())
+ if (not localPlan.has_value())
{
- ARMARX_VERBOSE << deactivateSpam(1) << "stack result invalid";
- // [[unlikely]]
- return;
+ ARMARX_INFO << "Local plan is invalid!";
+ srv.executor->stop();
}
@@ -822,25 +817,20 @@ namespace armarx::navigation::server
// ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
// << robotFrameVelocity.linear;
- srv.executor->execute(currentTrajectory());
+ srv.executor->execute(localPlan->trajectory);
}
void
- Navigator::updateIntrospector()
+ Navigator::updateIntrospector(const std::optional<loc_plan::LocalPlannerResult>& localPlan)
{
ARMARX_CHECK_NOT_NULL(srv.introspector);
- // TODO event driven. should be committed only on change
- // if(globalPlan.has_value())
- // {
- // srv.introspector->onGlobalPlannerResult(globalPlan.value());
- // }
-
- if (localPlan.has_value())
+ if (not localPlan.has_value())
{
- srv.introspector->onLocalPlannerResult(localPlan.value());
+ return;
}
+ srv.introspector->onLocalPlannerResult(localPlan.value());
}
void
@@ -858,9 +848,9 @@ namespace armarx::navigation::server
stop();
- srv.publisher->goalReached(
- core::GoalReachedEvent{{armarx::Clock::Now()},
- {core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())}});
+ srv.publisher->goalReached(core::GoalReachedEvent{
+ {armarx::Clock::Now()},
+ {core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())}});
srv.introspector->success();
@@ -876,49 +866,48 @@ namespace armarx::navigation::server
void
Navigator::stopAllThreads()
{
- stopIfRunning(replanningTask);
- stopIfRunning(monitorTask);
- }
-
- bool
- Navigator::isStackResultValid() const noexcept
- {
-
- // global planner
- if (config.stack.globalPlanner != nullptr)
- {
- if (not globalPlan.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Global trajectory not yet set.";
- return false;
- }
- }
-
- // local planner
- if (config.stack.localPlanner != nullptr)
- {
- if (not localPlan.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Local trajectory not yet set.";
- return false;
- }
- }
-
- // [[likely]]
- return true;
+ stopIfRunning(runningTask);
}
- const core::Trajectory& Navigator::currentTrajectory() const
- {
- ARMARX_CHECK(isStackResultValid());
-
- if(localPlan.has_value())
- {
- return localPlan->trajectory;
- }
-
- return globalPlan->trajectory;
- }
+ // bool
+ // Navigator::isStackResultValid() const noexcept
+ // {
+
+ // // global planner
+ // if (config.stack.globalPlanner != nullptr)
+ // {
+ // if (not globalPlan.has_value())
+ // {
+ // ARMARX_VERBOSE << deactivateSpam(1) << "Global trajectory not yet set.";
+ // return false;
+ // }
+ // }
+
+ // // local planner
+ // if (config.stack.localPlanner != nullptr)
+ // {
+ // if (not localPlan.has_value())
+ // {
+ // ARMARX_VERBOSE << deactivateSpam(1) << "Local trajectory not yet set.";
+ // return false;
+ // }
+ // }
+
+ // // [[likely]]
+ // return true;
+ // }
+
+ // const core::Trajectory& Navigator::currentTrajectory() const
+ // {
+ // ARMARX_CHECK(isStackResultValid());
+
+ // if(localPlan.has_value())
+ // {
+ // return localPlan->trajectory;
+ // }
+
+ // return globalPlan->trajectory;
+ // }
void
Navigator::pause()
@@ -937,6 +926,8 @@ namespace armarx::navigation::server
ARMARX_INFO << "Resume.";
executorEnabled.store(true);
+
+ srv.executor->start();
}
void
diff --git a/source/armarx/navigation/server/Navigator.h b/source/armarx/navigation/server/Navigator.h
index 19318c01e5228bad17d4ee8db97009df8cf6bd4f..a0affd5d1fad6ebbe6787e3132d65939e823266d 100644
--- a/source/armarx/navigation/server/Navigator.h
+++ b/source/armarx/navigation/server/Navigator.h
@@ -67,8 +67,7 @@ namespace armarx::navigation::server
{
struct
{
- int replanningUpdatePeriod{10}; // [ms]
- int monitorUpdatePeriod{10}; // [ms]
+ int replanningUpdatePeriod{100}; // [ms]
} tasks;
};
@@ -117,17 +116,18 @@ namespace armarx::navigation::server
void moveToAbsolute(const std::vector<core::Pose>& waypoints);
void moveTowardsAbsolute(const core::Direction& direction);
- void replan();
+ void run();
void updateScene();
- void updateLocalPlanner();
- void updateExecutor();
- void updateIntrospector();
+
+ std::optional<loc_plan::LocalPlannerResult> updateLocalPlanner();
+ void updateExecutor(const std::optional<loc_plan::LocalPlannerResult>& localPlan);
+ void updateIntrospector(const std::optional<loc_plan::LocalPlannerResult>& localPlan);
void updateMonitor();
- const core::Trajectory& currentTrajectory() const;
- bool isStackResultValid() const noexcept;
+ // const core::Trajectory& currentTrajectory() const;
+ // bool isStackResultValid() const noexcept;
void stopAllThreads();
@@ -137,6 +137,10 @@ namespace armarx::navigation::server
void setGraphEdgeCosts(core::Graph& graph) const;
GraphBuilder convertToGraph(const std::vector<client::WaypointTarget>& targets) const;
+ bool hasLocalPlanner() const noexcept
+ {
+ return config.stack.localPlanner != nullptr;
+ }
Config config;
@@ -144,8 +148,7 @@ namespace armarx::navigation::server
std::atomic_bool executorEnabled = true;
- PeriodicTask<Navigator>::pointer_type replanningTask;
- PeriodicTask<Navigator>::pointer_type monitorTask;
+ PeriodicTask<Navigator>::pointer_type runningTask;
std::optional<GoalReachedMonitor> goalReachedMonitor;
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
index 0882171f82e5c1d694bd76ac3c5eea11a6072b0b..a4dfe4c2343fd6585415907ee5c734a05e2316bd 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -9,8 +9,8 @@
#include <armarx/navigation/common/controller_types.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
-#include <armarx/navigation/platform_controller/controller_descriptions.h>
#include <armarx/navigation/platform_controller/aron_conversions.h>
+#include <armarx/navigation/platform_controller/controller_descriptions.h>
#include <armarx/navigation/platform_controller/json_conversions.h>
@@ -21,32 +21,47 @@ namespace armarx::navigation::server
ControllerComponentPlugin& controllerComponentPlugin) :
controllerPlugin(controllerComponentPlugin)
{
+ ARMARX_TRACE;
controllerComponentPlugin.getRobotUnitPlugin().getRobotUnit()->loadLibFromPackage(
"armarx_navigation", "libarmarx_navigation_platform_controller.so");
// make default configs available to the memory
ARMARX_INFO << "Loading default configs";
{
- const std::string configBasePath =
- PackagePath("armarx_navigation", "controller_config").toSystemPath();
+ // const std::filesystem::path configBasePath =
+ // PackagePath("armarx_navigation", "controller_config").toSystemPath();
- armarx::control::memory::config::parseAndStoreDefaultConfigs<
- armarx::navigation::common::ControllerType::PlatformTrajectory>(
- configBasePath, controllerComponentPlugin.configMemoryWriter());
+ ARMARX_TRACE;
+ // armarx::control::memory::config::parseAndStoreDefaultConfigs<
+ // armarx::navigation::common::ControllerType::PlatformTrajectory>(
+ // "" /*configBasePath*/, controllerComponentPlugin.configMemoryWriter());
+ ARMARX_TRACE;
ARMARX_INFO << "asdlfasfdlh";
}
// initialize controller
ARMARX_INFO << "Initializing controller";
{
+ ARMARX_TRACE;
auto builder = controllerPlugin.createControllerBuilder<
armarx::navigation::common::ControllerType::PlatformTrajectory>();
- auto ctrlWrapper = builder.create();
+ ARMARX_TRACE;
+ // FIXME remove withconfig
+ auto ctrlWrapper =
+ builder.withNodeSet("PlatformPlanning")
+ .withConfig(
+ "/home/fabi/workspace/armarx/skills/navigation/data/armarx_navigation/"
+ "controller_config/PlatformTrajectory/default.json")
+ .create();
+
+ ARMARX_TRACE;
+ ARMARX_CHECK(ctrlWrapper.has_value());
ctrl.emplace(std::move(ctrlWrapper.value()));
}
+ ARMARX_TRACE;
ARMARX_CHECK(ctrl.has_value());
ARMARX_INFO << "PlatformControllerExecutor: init done.";
}
@@ -57,6 +72,9 @@ namespace armarx::navigation::server
void
PlatformControllerExecutor::execute(const core::Trajectory& trajectory)
{
+ ARMARX_INFO << "Received trajectory for execution with " << trajectory.points().size()
+ << " points";
+
toAron(ctrl->config.targets.trajectory, trajectory);
// sends the updated config to the controller and stores it in the memory