diff --git a/source/armarx/navigation/server/NavigationStack.h b/source/armarx/navigation/server/NavigationStack.h
index 9c047123f4a851cb85448f5ba3424ac3a4e625be..c2a76f9a2269400f62d3e699d503036159db7a47 100644
--- a/source/armarx/navigation/server/NavigationStack.h
+++ b/source/armarx/navigation/server/NavigationStack.h
@@ -34,7 +34,7 @@ namespace armarx::navigation::server
{
global_planning::GlobalPlannerPtr globalPlanner;
loc_plan::LocalPlannerPtr localPlanner = nullptr;
- traj_ctrl::TrajectoryControllerPtr trajectoryControl;
- safe_ctrl::SafetyControllerPtr safetyControl = nullptr;
+ // traj_ctrl::TrajectoryControllerPtr trajectoryControl;
+ // safe_ctrl::SafetyControllerPtr safetyControl = nullptr;
};
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 3a1b42f2c58f29c5eb7164cbab8921c86055762e..bf343cba7e3d6d55e7a475eccbcd5df788a9865d 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -624,62 +624,18 @@ namespace armarx::navigation::server
ARMARX_TRACE;
ARMARX_INFO << "Planning local trajectory enabled";
- localPlannerTask =
+ replanningTask =
new PeriodicTask<Navigator>(this,
&Navigator::updateLocalPlanner,
- config.general.tasks.localPlanningUpdatePeriod,
+ config.general.tasks.replanningUpdatePeriod,
false,
"LocalPlannerTask");
- localPlannerTask->start();
+ replanningTask->start();
}
- // trajectory controller
- ARMARX_CHECK_NOT_NULL(config.stack.trajectoryControl);
- trajectoryControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateTrajectoryController,
- config.general.tasks.trajectoryControllerUpdatePeriod,
- false,
- "TrajectoryControllerTask");
- trajectoryControllerTask->start();
-
- // safety controller
- if (config.stack.safetyControl)
- {
- ARMARX_INFO << "Safety control enabled";
- safetyControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateSafetyController,
- config.general.tasks.safetyControllerUpdatePeriod,
- false,
- "SafetyControllerTask");
- safetyControllerTask->start();
- }
-
- // executor
- 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,
- false,
- "IntrospectorTask");
- introspectorTask->start();
// monitoring
ARMARX_INFO << "Starting monitoring";
- // std::vector<core::Pose> waypoints = globalPlan->trajectory.poses();
-
-
- // goalReachedMonitor =
- // GoalReachedMonitor(waypoints.back(), *config.scene, GoalReachedMonitorConfig());
monitorTask = new PeriodicTask<Navigator>(this,
&Navigator::updateMonitor,
config.general.tasks.monitorUpdatePeriod,
@@ -750,7 +706,7 @@ namespace armarx::navigation::server
ARMARX_TRACE;
srv.publisher->globalTrajectoryUpdated(globalPlan.value());
- // srv.introspector->onGlobalPlan(globalPlan.value());
+ srv.introspector->onGlobalPlannerResult(globalPlan.value());
ARMARX_INFO << "Global planning completed. Will now start all required threads";
ARMARX_TRACE;
@@ -759,58 +715,17 @@ namespace armarx::navigation::server
if (config.stack.localPlanner)
{
ARMARX_INFO << "Planning local trajectory enabled";
- localPlannerTask =
+ replanningTask =
new PeriodicTask<Navigator>(this,
- &Navigator::updateLocalPlanner,
- config.general.tasks.localPlanningUpdatePeriod,
+ &Navigator::replan,
+ config.general.tasks.replanningUpdatePeriod,
false,
"LocalPlannerTask");
- localPlannerTask->start();
+ replanningTask->start();
}
- // trajectory controller
- ARMARX_CHECK_NOT_NULL(config.stack.trajectoryControl);
- trajectoryControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateTrajectoryController,
- config.general.tasks.trajectoryControllerUpdatePeriod,
- false,
- "TrajectoryControllerTask");
- trajectoryControllerTask->start();
-
- // safety controller
- if (config.stack.safetyControl)
- {
- ARMARX_INFO << "Safety control enabled";
- safetyControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateSafetyController,
- config.general.tasks.safetyControllerUpdatePeriod,
- false,
- "SafetyControllerTask");
- safetyControllerTask->start();
- }
-
- // executor
- 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,
- false,
- "IntrospectorTask");
- introspectorTask->start();
-
// monitoring
ARMARX_INFO << "Starting monitoring";
-
monitorTask = new PeriodicTask<Navigator>(this,
&Navigator::updateMonitor,
config.general.tasks.monitorUpdatePeriod,
@@ -841,6 +756,14 @@ namespace armarx::navigation::server
{
}
+ void
+ Navigator::replan()
+ {
+ updateLocalPlanner();
+ updateExecutor();
+ updateIntrospector();
+ }
+
void
Navigator::updateLocalPlanner()
{
@@ -856,31 +779,6 @@ namespace armarx::navigation::server
}
}
- void
- Navigator::updateTrajectoryController()
- {
- // TODO distinguish between global and local plan!
-
- if (not globalPlan.has_value())
- {
- return;
- }
-
- trajectoryCtrlResult = config.stack.trajectoryControl->control(globalPlan->trajectory);
-
- srv.publisher->trajectoryControllerUpdated(trajectoryCtrlResult.value());
- }
-
- void
- Navigator::updateSafetyController()
- {
- // FIXME
- // if (res.controlVelocity)
- // {
- // res.safeVelocity = config.stack.safetyControl->control(res.controlVelocity.value());
- // }
- }
-
void
Navigator::updateExecutor()
{
@@ -898,32 +796,23 @@ namespace armarx::navigation::server
return;
}
- const core::Twist twist = [&]() -> core::Twist
- {
- if (config.stack.safetyControl != nullptr)
- {
- return safetyCtrlResult->twist;
- }
- return trajectoryCtrlResult->twist;
- }();
+ // const core::Rotation robot_R_world(config.scene->robot->getGlobalOrientation().inverse());
- 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;
- core::Twist robotFrameVelocity;
+ // robotFrameVelocity.linear = robot_R_world * twist.linear;
+ // // FIXME fix angular velocity
+ // robotFrameVelocity.angular = twist.angular;
- robotFrameVelocity.linear = robot_R_world * twist.linear;
- // FIXME fix angular velocity
- robotFrameVelocity.angular = twist.angular;
+ // ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
+ // << robotFrameVelocity.linear;
- ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
- << robotFrameVelocity.linear;
-
- srv.executor->move(robotFrameVelocity);
+ srv.executor->execute(currentTrajectory());
}
void
@@ -932,30 +821,16 @@ namespace armarx::navigation::server
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(globalPlan.has_value())
+ // {
+ // srv.introspector->onGlobalPlannerResult(globalPlan.value());
+ // }
if (localPlan.has_value())
{
srv.introspector->onLocalPlannerResult(localPlan.value());
}
- if (trajectoryCtrlResult.has_value())
- {
- srv.introspector->onTrajectoryControllerResult(trajectoryCtrlResult.value());
- }
-
- if (safetyCtrlResult.has_value())
- {
- srv.introspector->onSafetyControllerResult(safetyCtrlResult.value());
- }
-
- // if (res.isValid())
- // {
- // srv.introspector->onStackResult(res);
- // }
}
void
@@ -991,11 +866,7 @@ namespace armarx::navigation::server
void
Navigator::stopAllThreads()
{
- stopIfRunning(localPlannerTask);
- stopIfRunning(trajectoryControllerTask);
- stopIfRunning(safetyControllerTask);
- stopIfRunning(executorTask);
- stopIfRunning(introspectorTask);
+ stopIfRunning(replanningTask);
stopIfRunning(monitorTask);
}
@@ -1023,29 +894,21 @@ namespace armarx::navigation::server
}
}
- // trajectory controller
- if (config.stack.trajectoryControl != nullptr)
- {
- if (not trajectoryCtrlResult.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Raw control velocity not yet set.";
- return false;
- }
- }
+ // [[likely]]
+ return true;
+ }
- // safety controller
- if (config.stack.safetyControl != nullptr)
+ const core::Trajectory& Navigator::currentTrajectory() const
+ {
+ ARMARX_CHECK(isStackResultValid());
+
+ if(localPlan.has_value())
{
- if (not safetyCtrlResult.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Safe velocity not yet set.";
- return false;
- }
+ return localPlan->trajectory;
}
- // [[likely]]
- return true;
- }
+ return globalPlan->trajectory;
+ }
void
Navigator::pause()
@@ -1055,7 +918,7 @@ namespace armarx::navigation::server
executorEnabled.store(false);
ARMARX_CHECK_NOT_NULL(srv.executor);
- srv.executor->move(core::Twist::Zero());
+ srv.executor->stop();
}
void
diff --git a/source/armarx/navigation/server/Navigator.h b/source/armarx/navigation/server/Navigator.h
index a1ccfa3e3cc9cfd4ce9d4459fe7699931d48bd8f..933a1c24dd52d08d62e151684b0ff02fe79c46c6 100644
--- a/source/armarx/navigation/server/Navigator.h
+++ b/source/armarx/navigation/server/Navigator.h
@@ -62,13 +62,8 @@ namespace armarx::navigation::server
{
struct
{
- // TODO: Use chrono?
- int localPlanningUpdatePeriod{100}; // [ms]
- int trajectoryControllerUpdatePeriod{100}; // [ms]
- int safetyControllerUpdatePeriod{100}; // [ms]
- int introspectorUpdatePeriod{100}; // [ms]
- int executorUpdatePeriod{100}; // [ms]
- int monitorUpdatePeriod{20}; // [ms]
+ int replanningUpdatePeriod{10}; // [ms]
+ int monitorUpdatePeriod{10}; // [ms]
} tasks;
};
@@ -84,7 +79,6 @@ namespace armarx::navigation::server
IntrospectorInterface* introspector = nullptr;
};
- public:
Navigator(const Config& config, const InjectedServices& services);
void moveTo(const std::vector<core::Pose>& waypoints,
@@ -117,15 +111,15 @@ namespace armarx::navigation::server
void moveToAbsolute(const std::vector<core::Pose>& waypoints);
void moveTowardsAbsolute(const core::Direction& direction);
- void updateLocalPlanner();
- void updateTrajectoryController();
- void updateSafetyController();
+ void replan();
+ void updateLocalPlanner();
void updateExecutor();
-
void updateIntrospector();
+
void updateMonitor();
+ const core::Trajectory& currentTrajectory() const;
bool isStackResultValid() const noexcept;
void stopAllThreads();
@@ -143,16 +137,7 @@ namespace armarx::navigation::server
std::atomic_bool executorEnabled = true;
- // StackResult res;
-
- PeriodicTask<Navigator>::pointer_type localPlannerTask;
- PeriodicTask<Navigator>::pointer_type trajectoryControllerTask;
- PeriodicTask<Navigator>::pointer_type safetyControllerTask;
-
- PeriodicTask<Navigator>::pointer_type executorTask;
-
- PeriodicTask<Navigator>::pointer_type introspectorTask;
-
+ PeriodicTask<Navigator>::pointer_type replanningTask;
PeriodicTask<Navigator>::pointer_type monitorTask;
std::optional<GoalReachedMonitor> goalReachedMonitor;
@@ -160,8 +145,6 @@ namespace armarx::navigation::server
std::optional<global_planning::GlobalPlannerResult> globalPlan;
std::optional<loc_plan::LocalPlannerResult> localPlan;
- std::optional<traj_ctrl::TrajectoryControllerResult> trajectoryCtrlResult;
- std::optional<safe_ctrl::SafetyControllerResult> safetyCtrlResult;
};
} // namespace armarx::navigation::server