-
Notifications
You must be signed in to change notification settings - Fork 376
Dev_Action_Server_State_Machine_Model
This document provides an overview of the Action Server State Machine Model in both ROS1 and ROS2. It explains the state transitions, corresponding methods, and action statuses used in the implementation of an action server.
ROS2 Humble
- ROS Action Specification
- Corresponding Methods
- Action Status
- Client Triggered Transitions
- Server Triggered Transitions
- Server Methods
- Server Lifecycle
In ROS 2, the action server follows a state machine model similar to ROS 1 but with changes in terminology and implementation. The state transitions are defined by methods and managed via the ActionStatus
enum.
In ROS2 action design page, ROS defined the action server with a state machine model:
Image from ROS2 Action Design
Our ActionServer
implements the state machine model. Each transition in the state machine corresponds to a method, which updates the server's status and invokes user-defined behaviors.
Transition | Method | On transition method |
---|---|---|
Receive Goal | private void GoalCallback(actionGoal) |
protected abstract void OnGoalReceived() |
Cancel Request | private void CancelCallback(goalID) |
protected abstract void OnGoalCanceling() |
Set Accepted | protected void SetAccepted() |
protected abstract void OnGoalActive() |
Set Rejected | protected void SetRejected() |
protected virtual void OnGoalRejected() |
Set Succeeded | protected void SetSucceeded() |
protected virtual void OnGoalSucceeded() |
Set Aborted | protected void SetAborted() |
protected virtual void OnGoalAborted() |
Set Canceled | protected void SetCanceled() |
protected virtual void OnGoalCanceled() |
The ActionServer
uses the ActionStatus
enum, which represents the state of the server. The status can be:
-
STATUS_NO_GOAL
: Initial state, no goal received. -
STATUS_ACCEPTED
: Goal is accepted, waiting for processing. -
STATUS_EXECUTING
: Goal is actively being processed. -
STATUS_CANCELING
: Goal is being canceled. -
STATUS_SUCCEEDED
: Goal succeeded. -
STATUS_ABORTED
: Goal was aborted.
private void GoalCallback(TActionGoal actionGoal)
This method is triggered when the action server receives a new goal. The goal is saved in action.action_goal
, the server's status is set to STATUS_ACCEPTED
, and OnGoalReceived()
is called.
protected abstract void OnGoalReceived()
This method is implemented by the user to define custom behavior when a new goal is received.
private void CancelCallback(string frameId, string action)
This method is triggered when a cancel request is received from the client. It changes the server's status to STATUS_CANCELING
and calls OnGoalCanceling()
.
protected abstract void OnGoalCanceling()
This method is implemented by the user to define custom behavior when a goal is being canceled.
protected void SetAccepted()
This method is used to transition the server status to STATUS_ACCEPTED
from STATUS_NO_GOAL
. It then calls OnGoalActive()
, where the user can start processing the goal.
protected abstract void OnGoalActive()
This method is invoked when the server status is set to STATUS_ACCEPTED
. The user can implement this method to start goal processing.
protected void SetRejected()
This method is used to transition the server status to STATUS_REJECTED
. It then calls OnGoalRejected()
.
protected virtual void OnGoalRejected()
This method is invoked when the goal is rejected, typically used for error handling or logging.
protected void SetSucceeded()
This method sets the status to STATUS_SUCCEEDED
, indicating that the goal has been completed successfully. It then calls OnGoalSucceeded()
.
protected virtual void OnGoalSucceeded()
This method is invoked when the goal succeeds, and it can be used for result publishing or other completion tasks.
protected void SetAborted()
This method sets the status to STATUS_ABORTED
, indicating that the goal was aborted. It then calls OnGoalAborted()
.
protected virtual void OnGoalAborted()
This method is invoked when the goal is aborted, and it can be used to handle cleanup or other post-abort tasks.
protected void SetCanceled()
This method sets the status to STATUS_CANCELING
, indicating that the goal was canceled. It then calls OnGoalCanceled()
.
protected virtual void OnGoalCanceled()
This method is invoked when the goal is canceled. The user can implement custom behavior for goal cancellation.
protected void UpdateAndPublishStatus(ActionStatus actionStatus)
This method updates the current action status and publishes it to the appropriate topic. It allows the server to reflect its status changes.
protected void PublishFeedback()
This method publishes feedback about the goal’s progress.
protected void PublishResult()
This method publishes the result of the goal processing.
protected ActionStatus GetStatus()
This method returns the current ActionStatus
of the server, which can be used for monitoring or debugging.
protected virtual void Log(string log)
This method allows the user to override logging behavior for different log levels (info, warning, error).
-
Initialization: The
Initialize()
method starts the server by advertising the action via therosSocket.AdvertiseAction
method. -
Termination: The
Terminate()
method stops the server, unadvertises the action, and ensures that the server shuts down cleanly by setting the status toABORTED
if the goal was executing or canceling.
This revision ensures the terminology is consistent with the provided ROS 2 code structure and reflects the actual states and methods involved in the state transitions for the ActionServer
.
ROS1 Noetic
- ROS Action Specification
- Corresponding Methods
- Action Status 3.1. Client Triggered Transitions 3.2. Server Triggered Transitions 3.3. Server Methods 3.4. Server Lifecycle
In Actionlib Detailed Description, ROS defined the action server with a state machine model:
Image from ROS Actionlib Detailed Description
Please read the server section of Actionlib Detailed Description as our server is based on the ROS state machine model
Our ActionServer
implements the state machine model. Each transition in the state machine is represented by a method. which sets the status of the server and calls methods overridden by you that defines custom behavior after state transition is complete.
Transition | Representing Method | On transition method |
---|---|---|
Receive Goal | private void GoalCallback(actionGoal) |
protected abstract void OnGoalReceived() |
Cancel Request | private void CancelCallback(goalID) |
protected abstract void OnGoalRecalling(GoalID goalID) |
protected abstract void OnGoalPreempting() |
||
setAccepted | protected void SetAccepted() |
protected abstract void OnGoalActive() |
setRejected | protected void SetRejected() |
protected virtual void OnGoalRejected() |
setSucceeded | protected void SetSucceeded() |
protected virtual void OnGoalSucceeded() |
setAborted | protected void SetAborted() |
protected virtual void OnGoalAborted() |
setCanceled | protected void SetCanceled() |
protected virtual void OnGoalCanceled() |
Our ActionServer
uses ActionStatus
enum which corresponds to constant byte values in GoalStatus
Additionally, we have an ActionStatus
of NO_GOAL
which corresponds to server status before receiving a goal.
private void GoalCallback(TActionGoal actionGoal)
This method is called by WebSocket
subscription for <action name>/goal
topic. New goal gets stored in action.action_goal
and server state is set to PENDING
. Then, OnGoalReceived()
is called.
protected abstract void OnGoalReceived()
This method is called by GoalCallback(actionGoal)
after server saves goal and updates action status. Extension of ActionServer
are required to implement this method.
private void CancelCallback(GoalID goalID)
This method is called by WebSocket
subscription for <action name>/cancel
topic, sets action status to RECALLING
or PREEMPTING
and calls OnGoalRecalling(GoalID goalID)
or OnGoalPreempting()
respectively
protected abstract void OnGoalRecalling(GoalID goalID)
This method is called by CancelCallback(goalID)
when a goal is canceled during PENDING
action state. Extension of ActionServer
are required to implement this method.
protected abstract void OnGoalPreempting()
This method is called by CancelCallback(goalID)
when a goal is canceled during ACTIVE
action state. Extension of ActionServer
are required to implement this method. Generally used to signal goal processing thread to stop and join.
protected void SetAccepted(string text = "")
This method sets action status to ACTIVE
from PENDING
or to PREEMPTING
from RECALLING
. Invalid transitions are ignored and logged as a warning. Then OnGoalActive()
or OnGoalPreempting()
is called. Optionally, user can pass in a text for extra information in GoalStatus
protected abstract void OnGoalActive()
This method is called by SetAccepted()
after action status is set to ACTIVE
. Extension of ActionServer
are required to implement this method, and is generally used to start a new thread for goal processing.
protected void SetRejected(string text = "")
This method sets action status to REJECTED
from PENDING
or RECALLING
. Invalid transitions are ignored and logged as a warning. Then OnGoalRejected()
is called. Optionally, user can pass in a text for extra information in GoalStatus
protected virtual void OnGoalRejected() { }
This method is called by SetRejected()
after action status is set to REJECTED
and doesn't do anything by default.
protected void SetSucceeded(TResult result = null, string text = "")
This method sets action status to SUCCEEDED
from ACTIVE
or PREEMPTING
. Invalid transitions are ignored and logged as a warning. If user chooses to pass in an optional Result
, action.action_result.result
is updated. The result is then published and OnGoalSucceeded()
is called. Optionally, user can pass in a text for extra information in GoalStatus
protected virtual void OnGoalSucceeded() { }
This method is called by SetSucceeded()
after action status is set to SUCCEEDED
and doesn't do anything by default.
protected void SetAborted(string text = "")
This method sets action status to ABORTED
from ACTIVE
or PREEMPTING
. Invalid transitions are ignored and logged as a warning. Then OnGoalAborted()
is called. Optionally, user can pass in a text for extra information in GoalStatus
protected virtual void OnGoalAborted() { }
This method is called by SetAborted()
after action status is set to ABORTED
and doesn't do anything by default.
protected void SetCanceled(TResult result = null, string text = "")
This method sets action status to RECALLED
from RECALLING
or PREEMPTED
from PREEMPTING
. Invalid transitions are ignored and logged as a warning. If user chooses to pass in an optional Result
, action.action_result.result
is updated. Then OnGoalCanceled()
is called. Optionally, user can pass in a text for extra information in GoalStatus
protected virtual void OnGoalCanceled() { }
This method is called by SetCanceled()
after action status is set to RECALLED
or PREEMPTED
and doesn't do anything by default.
protected void UpdateAndPublishStatus(ActionStatus actionStatus, string text = "")
Updates current action status and calls PublishStatus()
. Optionally, user can pass in a text for extra information in GoalStatus
. Generally not called in extension classes to prevent illegal state transitions.
protected void PublishStatus()
Publishes current action status as the first element of a GoalStatusArray
to <action name>/status
topic. If current status is NO_GOAL
, an empty array is published.
protected void PublishFeedback()
Publishes current stored action.action_feedback
with current ActionStatus
and GoalID
and publishes feedback to <action name>/feedback
topic.
protected void PublishResult()
Publishes current stored action.action_result
with current ActionStatus
and GoalID
and publishes feedback to <action name>/feedback
topic.
protected ActionStatus GetStatus()
Returns the current ActionStatus
. Note that this will return NO_GOAL
which is not a part of ROS standard GoalStatus
.
protected virtual void Log(string log) { }
/protected virtual void LogWarning(string log) { }
/protected virtual void LogError(string log) { }
Allows user to override to direct log messages to appropriate streams. Doesn't do anything by default.
On construction, the server connects to provided serverURL
.
To start advertising and subscribing, please call Start()
. Note that Start()
doesn't spin the server and without a spinning method, server will terminate.
Stop()
will unadvertise and unsubscribe all topics and close the socket it is using.
© Siemens AG, 2017-2025
-
- 1.3.1 R2D2 Setup
- 1.3.2 Gazebo Setup on VM
- 1.3.3 TurtleBot Setup (Optional for ROS2)
- 2.1 Quick Start
- 2.2 Transfer a URDF from ROS to Unity
- 2.3 Transfer a URDF from Unity to ROS
- 2.4 Unity Simulation Scene Example
- 2.5 Gazebo Simulation Scene Example
- 2.6 Fibonacci Action Client
- 2.7 Fibonacci Action Server
- 3.1 Import a URDF on Windows
- 3.2 Create, Modify and Export a URDF Model
- 3.3 Animate a Robot Model in Unity
- 4.1 Introduction to RosBridgeClient
- 4.2 Image Publication
- 4.3 URDF Transfer
- 4.4 Fibonacci Action Client/Server
- Message Handling: Readers & Writers
- Thread Safety for Message Reception
- File Server Package
- ROS-Unity Coordinate System Conversions
- Post Build Events
- Preprocessor Directives in ROS#
- Adding New Message Types
- RosBridgeClient Protocols
- RosBridgeClient Serializers
- Actions in ROS#
- Action Server State Machine Model
© Siemens AG, 2017-2025