## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links
## note.0=
## descriptive title for the tutorial
## title = System Overview
## multi-line description to be displayed in search
## description = This chapter provides a deep insight of the system architectur. Reading this tutorial will help to understand how the step controller works under the hood and thus, enables the reader to setup other robot systems.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for
## level= BeginnerCategory
## keywords =
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

## AUTOGENERATED DO NOT DELETE
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE

<<TOC(3)>>

== Overview ==

The vigir_step_controller is designed to provide easy access to high-level online footstep planning and execution capabilities. In order to achieve online footstep planning, a [[http://docs.ros.org/kinetic/api/vigir_step_control/html/classvigir__step__control_1_1StepQueue.html|StepQueue]] is maintaining all received step plans. It is able to update single steps, merge and stitch entire step plans.

Furthermore, an integrated state machine is tracking the current walking progress and executes steps from the step queue using a dedicated plugin interfacing the robot's motion layer.

== ROS API ==

{{{
#!clearsilver CS/NodeAPI
sub {
  0.name = vigir_step_controller/load_step_plan_msg_plugin
  0.type = std_msgs/String
  0.desc = Loads !StepPlanMsgPlugin converting generic step plan messages into robot speicific format
  1.name = vigir_step_controller/load_step_controller_plugin
  1.type = std_msgs/String
  1.desc = Loads step controller plugin implementing low-level interfaces for the motion layer
  2.name = vigir_step_controller/execute_step_plan
  2.type = vigir_footstep_planning_msgs/StepPlan
  2.desc = All step plan published at this topic are added to the step queue for execution
}
pub {
  0.name = vigir_step_controller/execute_feedback
  0.type = vigir_footstep_planning_msgs/ExecuteStepPlanFeedback
  0.desc = Topic publishing step execution feedbacks
}
goal {
  0.name = vigir_step_controller/execute_step_plan
  0.type = vigir_footstep_planning_msgs/ExecuteStepPlanGoal
  0.desc = Action server for step plan execution
}
}}}

== Plugins ==

The step controller relies on two plugins based on [[vigir_pluginlib]] explained next. Those plugins are loaded at startup of the system but can be replaced during runtime.

=== StepPlanMsgPlugin ===

Motion algorithms require often detailed information for step execution. During footstep planning, this plugin is used to decode optional additional custom data into the step plan (see [[vigir_footstep_planning]] package). For this reason, the step controller relies on this plugin to decode all this custom data if available from the step plan during footstep execution. Furthermore, this plugin can be used to provide proper algorithms to convert the generic step data into robot/algorithm-specific format.

However, if the motion algorithm does not require any custom data, then the provided default implementation (namely !StepPlanMsgPlugin) is sufficient.

=== StepControllerPlugin ===

The !StepControllerPlugin defines a bottom-up designed interface for the step controller to interface the robot's motion layer directly. Therefore, for every robot/motion algorithm, such a plugin must be implemented. Through this plugin, the user is also able to (re-)implement the entire behavior (state machine) of the controller and with the help of the !StepPlanMsgPlugin to perform all required conversion from high-level planned steps to low-level motion commands.

For this purpose, it defines multiple methods called during an update iteration as described in [[#callback_order|Callback Order]]) in detail. It is also responsible for providing proper state transitions for the controller's state machine.

The !StepControllerPlugin partially pre-implements a basic common state machine (see [[#state_machine|State Machine]]) for proper step execution. Therefore, each plugin needs at minimum to implement following methods
 * preProcess(...)
 * initWalk(...)
 * executeStep(...)
as described next.

The full details of the provided interface are documented in the [[http://docs.ros.org/kinetic/api/vigir_step_control/html/classvigir__step__control_1_1StepControllerPlugin.html|code API of the StepControllerPlugin]]

<<Anchor(callback_order)>>
== Callback Order ==

The [[http://docs.ros.org/kinetic/api/vigir_step_control/html/classvigir__step__control_1_1StepController.html|StepController]] is responsible for interfacing the ROS API and calling the step controller's update loop with a fixed rate (default 10Hz).

{{attachment:step_controller_update.png||width=750}}

The update loop (see image above) consists of three processing steps based on virtual function calls to the used !StepControllerPlugin and an additional feedback call publishing the feedback messages in between. The intended purpose and pre-implemented functionality of each processing step are described in detail next.

=== preProcess(...) ===

Before any steps should be processed, the `preProcess(...)` method has to pre-check the current robot and walking condition. It shall be checked if the motion layer has detected any errors and everything is still working as expected. Furthermore, the step plan execution progress should be supervised in this step. This includes checking and triggering of step execution (fetching steps from the queue, see [[#execution_progression|Execution Progression]]) as well as determining successful execution of the entire step plan. If the step plan has been completely executed, the controller's state must be set to `FINISHED` here.

==== initWalk(...) ====

This method should implement all required initialization to enable the use of the motion algorithms and controllers. If a new step plan is received while the controller is in `READY` state, it will be automatically called by `preProcess(...)` using the default !StepControllerPlugin implementation. It will not be triggered again during an already started motion sequence.

=== process(...) ===

The `process(...)` method is responsible for checking if a single or even multiple steps must be forwarded to the motion layer (triggered by `preProcess(...)`). Each required step can be easily obtained from the step queue and must be given as argument to an `executeStep(...)` callback. This method must not manage the step queue (i.e. step plan updates) as it is already done by the controller. The default implementation of `process(...)` can already handle step playback well and should be sufficient in most cases.

==== executeStep(...) ====

As soon the step controller has detected that the motion algorithm requires the next step data, this method is called by `process(...)`. Hence, this method is only responsible for forwarding the given step to the motion layer performing all required data conversions and calling the low-level motion layer. The default `process(...)` implementation does already contain the required logic to trigger the execution callback which is explained deeper in the [[#execution_progression|Execution Progression]] section.

=== postProcess(...) ===

During the `postProcess(...)` callback all cleanups, data logging and walking status checking should be performed.

<<Anchor(state_machine)>>
== State Machine ==

{{attachment:step_controller_fsm.png||width=750}}

The state machine transitions should be triggered within the `preProcess(...)`, `process(...)` and `postProcess(...)` calls by individually implemented logic as required to run the motion layer properly.

=== Abstract Transitions ===

The !StepControllerPlugin implements already an abstract state machine behavior illustrated above. It is called abstract as not all displayed transition are pre-implemented: All transitions labeled with `setState(state)` have to be triggered by the user implementation, calling the `setState(...)` method with the proposed argument at the appropriate place in any of the callback methods introduced above.

It is also open to the user to add or replace transition triggers to the pre-implemented state machine.

<<Anchor(execution_progression)>>
=== Execution Progression ===

The default implemented behavior in !StepControllerPlugin is using the step indices to check if new steps have to be executed. Each time the condition `getLastStepIndexSent() < getNextStepIndexNeeded()` evaluates to true the next step is taken from the queue. Therefore triggering a step execution event becomes very simple. As soon the motion layer requests a new step, the `setNextStepIndexNeeded(index)` has to be called with the proper step index argument. Usually, this can happen in the `preProcess(...)` step.

On the other side `setLastStepIndexSent(index)` must be set to the corresponding value when the step has been forwarded to the motion layer to stop triggering duplicated step execution callbacks. The default !StepControllerPlugin implementation takes already care about this.

Those presented fields can be used by cross checking the step queue to determine if the step plan execution has been completed and thus, the state machine can progress to the `FINISHED` state.

==== Feedback ====

The step controller does also provide feedback for the high-level system. For this purpose the feedback state must be updated continuously, e.g. by calling `updateQueueFeedback()` as soon changes have been applied to the step queue. The default !StepControllerPlugin implementation does already cover all cases. But if any custom code does further modifications on the step queue, the programmer should consider updating the feedback as well.

In order to report step execution progress in the feedback message, this information has to be updated as well. This can be achieved by updating the corresponding fields in the feedback message directly. A typical example is given in the code snippet below demonstrating the incrementation of all values after successful execution of a single step.
{{{
#!cplusplus
msgs::ExecuteStepPlanFeedback feedback = getFeedbackState();

feedback.header.stamp = ros::Time::now();

feedback.last_performed_step_index++;
feedback.currently_executing_step_index++;
feedback.first_changeable_step_index++;

setFeedbackState(feedback);
}}}

'''Note:''' Feedback has to be updated during `preProcess(...)` and `process(...)` calls. Changes during `postProcess(...)` become only visible in the next iteration and may be overwritten by an updated state.

== Examples ==

The [[https://github.com/team-vigir/vigir_step_control/blob/master/src/step_controller_test_plugin.cpp|StepControllerTestPlugin]] provides already a basic fully working test implementation to demonstrate the usage of the system (see [[vigir_step_control#example_startup|Example Startup]]). This plugin does just simulate the execution of single steps.

A real world [[https://github.com/thor-mang/ROBOTIS-THORMANG-MPC/blob/indigo-devel/thormang3_step_control_module/src/robotis_online_walking_plugin.cpp|example]] can be found in [[https://github.com/thor-mang/thor_mang_install|our THORMANG3 setup]].