[Documentation] [TitleIndex] [WordIndex

robot_statemachine: rsm_additions | rsm_core | rsm_msgs | rsm_rqt_plugins | rsm_rviz_plugins

Package Summary

The statemachine package includes the Robot Statemachine's main functionality


The RSM consists of various non-customizable and custom states that are based on the Base State. The former non-customizable states and the Base State are a part of this package containing the RSM's basics. To handle state transitions the State Interface is used. Robot Control Mux coordinates the actual control of the robot's movement while the Service Provider contains services, publishers and subscribers for communication between states and updating the GUI. To be able to handle arbitrary robots, the RSM relies on Plugins that can be implemented depending on the robot.

Base State

The base state for all states of the RSM features the following four main functions:

The function onSetup is called immediately after it was constructed and should be used to initialize the state. The function onEntry is run before the state's onActive method is executed for the first time and should be used to start up the processing in the state. The latter is the state's primary method that is executed periodically and contains its main logic. onExit is called before the state will be destroyed and should take care of leaving the state cleanly.

To realize interrupts in the RSM, the following five functions need to be implemented:

These functions handle commands issued from the GUI or the use of teleoperation by telling the current state which command or interrupt occurred and let the state handle it. The method onInterrupt receives the type of interrupt which are also defined in the Base State and are listed below:

The four other methods receive a reference to a bool and a string variable. The former informs if the request was successful and the desired action will be executed (true) or not (false) and the latter features a descriptive text.

The Base State holds a reference to the State Interface which has to be used for state transitions. It also has a variable with its name that is necessary to display the current state in the GUI and needs to be set in the onSetup or onEntry method.

State Interface

The State Interface holds a reference to the current state and handles state transitions. It also provides references to plugins created for exploration, navigation, mapping or routines. The State Interface provides the method transitionToVolatileState which will initiate a transition to the state provided as an argument. The provided argument is a boost::shared_ptr of the Base State type. This can be one of the known non-customizable states or a custom state defined through a plugin.

To access these plugins State Interface offers the method getPluginState which takes the plugin type and optionally a plugin routine name as parameters. The former can be one of the following types:

For a ROUTINE_STATE the routine name needs to be provided as well, otherwise this parameter can remain empty. The other plugin states are set by parameters provided to State Interface on launch. If no plugin type was specified but only a name, arbitrary plugins can be created and returned for state transition. If no plugin type and name were received or the desired plugin to be created does not exist, the Idle State will be returned and an error message put out.

State Interface subscribes to the stateInfo and simpleGoal topics to issue interrupts to the currently active state. Furthermore, it offers the two services startStopExploration and startStopWaypointFollowing which call the particular function in the active state.

The State Interface updates the currently active state periodically through its awake function. This function also executes the state transition initiated by transitionToVolatileState and calls the active state's methods.

Robot Control Mux

The Robot Control Mux (=Multiplexer) controls the velocity commands sent to the ROS node interfacing the motor controllers. In a simple configuration, a navigation or teleoperation node would output velocity commands that will be received by the motor controller interface and move the robot. To enable high level control of the input the motor controller receives, the Robot Control Mux should be the only node in the setup publishing directly on the topic the motor controller interface subscribes to.

Velocity commands generated by navigation should be published to an autonomy topic and velocity commands issued by teleoperation to a teleoperation topic. These two topics are subscribed by the Robot Control Mux that decides which or if any topic will be forwarded. The two input and the output topic's names are set by parameters at launch.

Which topic will be conducted is based on the operation mode which can be one of the following:

For Autonomy and Teleoperation the respective topic is propagated to the motor controller interface. If the operation mode is set to Stopped a command velocity of zero for all directions is published. The operation mode can be set through the GUI by a service Robot Control Mux is providing. It is published to the GUI for display as well. If a teleoperation command is issued, the mode automatically switches to Teleoperation. When in Teleoperation mode, a timer is started to supervise if new commands are being issued. If no new commands are received for the timer duration (which is set through a parameter), Teleoperation is replaced with the Stopped mode.

If the software emergency stop is activated in the GUI, the operation mode is handled as Stopped and cannot be changed until the stop button is released again.

Service Provider

The Service Provider handles the communication between the different states and saves data throughout state transitions. Therefore it offers a lot of services to save and retrieve variables for the core functionality of the RSM.

It offers all services to control waypoint following which includes adding, moving and removing single waypoints, setting their visited and unreachable variables and the routine to be executed upon reaching the waypoint. Furthermore, all waypoints can be retrieved and reset which effectively sets visited and unreachable to false. The waypoint following mode can be set and the list of all available routines retrieved. The latter is given as a parameter to the Service Provider. The list of waypoints is also published.

For setting and retrieving the current navigation goal the Service Provider is offering services. In addition, previously failed goals can be set, retrieved or reset. These serve as a way of blacklisting goals.

The current robot pose can be retrieved and is calculated from the transform of the map to the robot's base footprint.

The Service Provider hosts services for exploration that enable setting and getting the exploration mode. It is also published. If the exploration mode is set to Interrupt, the Service Provider subscribes to the list of available exploration goals and checks if the current navigation goal is still in this list. A tolerance for comparing these positions can be set with a parameter. If the navigation goal is not an exploration goal anymore, it becomes obsolete. This info is published when the mode is set to Interrupt as well.

Furthermore, it advertises services for setting and retrieving the reverse mode, which is also published.

Non-customizable states

The core state machine already features the following states for direct usage:


The RSM package requires three different plugin states, one for exploration to calculate the next goal, one for navigation and one for mapping. The first is called when exploration is started or a previous exploration target was mapped successfully and should interface an exploration package like explore_lite which finds unexplored regions in the map and extract a next goal from it. The second should interface a package for navigation like the ROS navigation stack and update the RSM according to the navigation's progress. The last is called when an exploration goal is reached and can include movements for better map acquisition or similar behaviors.

Also, up to ten plugins states can be included for the waypoint following routines that are executed upon reaching a waypoint. They are not necessary for the RSM like the plugins mentioned above. These routines can be implemented to enable arbitrary behavior when reaching a certain waypoint, for example inspecting gauge valves with a camera.

More plugins can be added if additional states during exploration or waypoint following are desired. These can only be called from other implemented plugin states as the basic RSM only includes transitions to the plugins described above. For example, if you have a robot able to climb stairs and you detect stairs during navigation, you can then call another plugin for stair-climbing and afterwards transition back to normal navigation.


The following section displays some examples and tutorials on how to use the RSMas well as the required setup to use the RSM. Afterwards, running and launching the RSM on its own and in a simulation environment is presented. The provided GUI and its controls are shown and a tutorial on writing and including your own plugin state is presented last.

  1. Launch RSM Simulation

    How to launch the provided simulations demonstrating the Robot Statemachine.

  2. GUI Introduction

    How to use the GUI provided by the Robot Statemachine in RViz and rqt.

  3. RSM Robot Setup

    Required steps to setup a mobile robot to use the Robot Statemachine for exploration and waypoint following.

  4. Run RSM

    Launch the Robot Statemachine for your robot

  5. Writing a Plugin State

    How to write a plugin state for the Robot Statemachine and properly setup the package where it will be in

  6. Use Plugin State in the RSM

    Use the previously created Plugin State in the RSM



This node realizes transitions between the different states.

Subscribed Topics

operationMode (rsm_msgs/OperationMode) simpleGoal (geometry_msgs/PoseStamped)

Published Topics

stateInfo (std_msgs/String)


startStopExploration (std_srvs/SetBool) startStopWaypointFollowing (std_srvs/SetBool) stateInfo (std_srvs/Trigger) stop2DNavGoal (std_srvs/Trigger)


~update_frequency (float, default: 20) ~calculate_goal_plugin (string, default: "rsm::CalculateGoalPlugin") ~navigation_plugin (string, default: "rsm::NavigationPlugin") ~mapping_plugin (string, default: "rsm::MappingPlugin")


This node is for controlling if the robot is running autonomous, by teleoperation or is stopped.

Subscribed Topics

<teleoperation_cmd_vel_topic> (std_msgs/String) <autonomy_operation_cmd_vel_topic> (std_msgs/String)

Published Topics

<cmd_vel_topic> (std_msgs/String) operationMode (rsm_msgs/OperationMode)


setOperationMode (rsm_msgs/SetOperationMode)


~update_frequency (float, default: 20) ~autonomy_cmd_vel_topic (string, default: "autonomy/cmd_vel") ~teleoperation_cmd_vel_topic (string, default: "teleoperation/cmd_vel") ~cmd_vel_topic (string, default: "cmd_vel") ~teleoperation_idle_timer (double, default: 0.5)


This node provides services for saving and receiving data needed by the volatile states.

Subscribed Topics

exploration_goals (geometry_msgs/PoseArray)

Published Topics

waypoints (rsm_msgs/WaypointArray) explorationMode (std_msgs/Bool) goalObsolete (std_msgs/Bool) reverseMode (std_msgs/Bool)


addWaypoint (rsm_msgs/AddWaypoint) getWaypoints (rsm_msgs/GetWaypoints) moveWaypoint (rsm_msgs/MoveWaypoint) removeWaypoint (rsm_msgs/RemoveWaypoint) waypointVisited (rsm_msgs/WaypointVisited) waypointUnreachable (rsm_msgs/WaypointUnreachable) resetWaypoints (std_srvs/Trigger) setWaypointFollowingMode (rsm_msgs/SetWaypointFollowingMode) setWaypointRoutine (rsm_msgs/SetWaypointRoutine) getWaypointRoutines (rsm_msgs/GetWaypointRoutines) setNavigationGoal (rsm_msgs/SetNavigationGoal) getNavigationGoal (rsm_msgs/GetNavigationGoal) addFailedGoal (rsm_msgs/AddFailedGoal) getFailedGoals (rsm_msgs/GetFailedGoals) resetFailedGoals (std_srvs/Trigger) getRobotPose (rsm_msgs/GetRobotPose) setExplorationMode (std_srvs/SetBool) getExplorationMode (std_srvs/Trigger) SetReverseMode (std_srvs/SetBool) GetReverseMode (std_srvs/Trigger)


~update_frequency (float, default: 20) ~robot_frame (string, default: "/base_link") ~waypoint_routines (std::vector, default: []) ~exploration_goal_tolerance (double, default: 0.05)

Required tf Transforms


2020-01-18 13:09