Show EOL distros:
Package Summary
The statemachine package includes the Robot Statemachine's main functionality
- Maintainer status: maintained
- Maintainer: Marco Steinbrink <marco.stb1993 AT gmail DOT com>
- Author: Marco Steinbrink <marco.stb1993 AT gmail DOT com>
- License: BSD
- Bug / feature tracker: https://github.com/MarcoStb1993/robot_statemachine/issues
- Source: git https://github.com/MarcoStb1993/robot_statemachine.git (branch: master)
Package Summary
The statemachine package includes the Robot Statemachine's main functionality
- Maintainer status: maintained
- Maintainer: Marco Steinbrink <marco.stb1993 AT gmail DOT com>
- Author: Marco Steinbrink <marco.stb1993 AT gmail DOT com>
- License: BSD
- Bug / feature tracker: https://github.com/MarcoStb1993/robot_statemachine/issues
- Source: git https://github.com/MarcoStb1993/robot_statemachine.git (branch: melodic-devel)
Contents
There are breaking changes for exploration goal handling between kinetic and melodic!
Documentation
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:
- onSetup
- onEntry
- onActive
- onExit
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:
- onExplorationStart
- onExplorationStop
- onWaypointFollowingStart
- onWaypointFollowingStop
- onInterrupt
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:
- INTERRUPT_END: Former interrupt ended (only relevant to specific interrupt handlers)
- EMERGENCY_STOP_INTERRUPT: Emergency Software Stop was pushed in the GUI
- TELEOPERATION_INTERRUPT: Teleoperation was used
- SIMPLE_GOAL_INTERRUPT: A navigation goal was issued through the RViz GUI
- SIMPLE_GOAL_STOP_INTERRUPT: The navigation goal was stopped in the GUI
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:
- CALCULATEGOAL_STATE
- NAVIGATION_STATE
- MAPPING_STATE
- ROUTINE_STATE
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:
- Autonomy
- Stopped
- Teleoperation
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:
- Boot State: Is the first state to be called and subscribes to a service which tells it when all necessary systems are available and ready to use. Then it initiates a transition to the Idle State. Can only be interrupted by the software emergency stop.
- Emergency Stop State: State being called when the software emergency stop was pushed. Only allows transition to Idle State when button is released.
- Idle State: Standard state when no commands were issued. Allows transitions to all other states through interrupts.
- Teleoperation State: State being called when teleoperation commands were issued. Only transitions to Idle State when teleoperation timed out and Emergency Stop State when receiving the respective interrupt.
- Waypoint Following State: Handles the waypoint following functionality by providing the next navigation goal depending on the status of all waypoints and the waypoint following mode. Normally transitions to the navigation state plugin. Can be interrupted by the software emergency stop and teleoperation which leads to a transition to the particular state. If waypoint following is stopped, transitions to Idle State.
Plugins
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.
Tutorials
The following section displays some examples and tutorials on how to use the RSM as 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.
- Launch RSM Simulation
How to launch the provided simulations demonstrating the Robot Statemachine.
- GUI Introduction
How to use the GUI provided by the Robot Statemachine in RViz and rqt.
- RSM Robot Setup
Required steps to setup a mobile robot to use the Robot Statemachine for exploration and waypoint following.
- Run RSM
Launch the Robot Statemachine for your robot
- Writing a Plugin State
How to write a plugin state for the Robot Statemachine and properly setup the package where it will be in
- Use Plugin State in the RSM
Use the previously created Plugin State in the RSM
Base State
The base state for all states of the RSM features the following four main functions:
- onSetup
- onEntry
- onActive
- onExit
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:
- onExplorationStart
- onExplorationStop
- onWaypointFollowingStart
- onWaypointFollowingStop
- onInterrupt
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:
- INTERRUPT_END: Former interrupt ended (only relevant to specific interrupt handlers)
- EMERGENCY_STOP_INTERRUPT: Emergency Software Stop was pushed in the GUI
- TELEOPERATION_INTERRUPT: Teleoperation was used
- SIMPLE_GOAL_INTERRUPT: A navigation goal was issued through the RViz GUI
- SIMPLE_GOAL_STOP_INTERRUPT: The navigation goal was stopped in the GUI
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:
- CALCULATEGOAL_STATE
- NAVIGATION_STATE
- MAPPING_STATE
- ROUTINE_STATE
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:
- Autonomy
- Stopped
- Teleoperation
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. The same functionality can also be activated for commands from a joystick by setting the respective parameters.
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, the current robot pose can be retrieved and is calculated from the transform of the map to the robot's base footprint. Furthermore, when a navigation goal is completed, a respective service must be called that handles waypoint following and exploration for a successful or unsuccessful completion.
The Service Provider hosts services for exploration that enable setting and getting the exploration mode. It is also published. When the mode is set to Interrupt, goals can become obsolete. This means the exploration algorithm has found more rewarding goals to go to.
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:
- Boot State: Is the first state to be called and subscribes to a service which tells it when all necessary systems are available and ready to use. Then it initiates a transition to the Idle State. Can only be interrupted by the software emergency stop.
- Emergency Stop State: State being called when the software emergency stop was pushed. Only allows transition to Idle State when button is released.
- Idle State: Standard state when no commands were issued. Allows transitions to all other states through interrupts.
- Teleoperation State: State being called when teleoperation commands were issued. Only transitions to Idle State when teleoperation timed out and Emergency Stop State when receiving the respective interrupt.
- Waypoint Following State: Handles the waypoint following functionality by providing the next navigation goal depending on the status of all waypoints and the waypoint following mode. Normally transitions to the navigation state plugin. Can be interrupted by the software emergency stop and teleoperation which leads to a transition to the particular state. If waypoint following is stopped, transitions to Idle State.
Plugins
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.
Tutorials
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.
- Launch RSM Simulation
How to launch the provided simulations demonstrating the Robot Statemachine.
- GUI Introduction
How to use the GUI provided by the Robot Statemachine in RViz and rqt.
- RSM Robot Setup
Required steps to setup a mobile robot to use the Robot Statemachine for exploration and waypoint following.
- Run RSM
Launch the Robot Statemachine for your robot
- Writing a Plugin State
How to write a plugin state for the Robot Statemachine and properly setup the package where it will be in
- Use Plugin State in the RSM
Use the previously created Plugin State in the RSM
Nodes
rsmNode
This node realizes transitions between the different states.Subscribed Topics
operationMode (rsm_msgs/OperationMode)- The current operation mode as set by the GUI or interrupts
- Navigation goal set in RViz
Published Topics
stateInfo (std_msgs/String)- Current state info text
Services
startStopExploration (std_srvs/SetBool)- Call to start or stop exploration, depending on the bool value (true: start, false: stop)
- Call to start or stop waypoint following, depending on the bool value (true: start, false: stop)
- Get current state info text
- Call to stop navigation started through RViz tool 2D Nav Goal
Parameters
~update_frequency (float, default: 20)- Update rate in Hz
- Sets the plugin's name for the state calculating the next goal
- Sets the plugin's name for the navigation state
- Sets the plugin's name for the mapping state.
robotControlMuxNode
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)- Publishes teleoperation command velocities
- Publishes autonomy command velocities
Published Topics
<cmd_vel_topic> (std_msgs/String)- Command velocity that the robot should follow
- The current operation mode as set by the GUI or interrupts
Services
setOperationMode (rsm_msgs/SetOperationMode)- Sets the operation mode to the given parameter
Parameters
~update_frequency (float, default: 20)- Update rate in Hz
- Topic name for the autonomy command velocity
- Topic name for the teleoperation command velocity
- Topic name for the command velocity the robot should follow
- Time until teleoperation is stopped when no new command is received
serviceProviderNode
This node provides services for saving and receiving data needed by the volatile states.Subscribed Topics
exploration_goals (geometry_msgs/PoseArray)- List of all currently available exploration goals (only active if exploration mode is set to "interrupt")
Published Topics
waypoints (rsm_msgs/WaypointArray)- List of all waypoints and their information
- The current exploration mode (true: interrupt, false: finish)
- Information if the current goal is still viable (only active is exploration mode is set to "interrupt")
- Information if the robot is currently moving in reverse (true: reverse, false: forward)
Services
addWaypoint (rsm_msgs/AddWaypoint)- Add a waypoint to the list of waypoints
- Get list of waypoints
- Move the waypoint at the given position in the waypoint list
- Remove the waypoint at the given position in the waypoint list
- Set the waypoint at the given position in the waypoint list to visited
- Set the waypoint at the given position in the waypoint list to unreachable
- Reset all waypoint's status to default
- Sets the waypoint following mode (0: single, 1: roundtrip, 2: patrol)
- Sets the routine of the waypoint at the given position in the waypoint list
- Return the list of all waypoint routines available
- Sets the current navigation goal
- Gets the current navigation goal
- Add an unreachable goal to the list of all failed goals
- Return list of all previously failed goals
- Deletes the list of previously failed goals
- Return the current robot pose in relation to the map frame
- Set the exploration mode (true: interrupt, false: finish)
- Get the exploration mode
- Set the robot to reverse mode (true: reverse, false: forward)
- Return the reverse mode (true: reverse, false: forward)
Parameters
~update_frequency (float, default: 20)- Update rate in Hz
- Transform frame for the robot
- Vector with waypoint routines available as state plugins
- Distance in all directions in meters that the robot's current position can differ from an exploration goal to still count it as reached
Required tf Transforms
<robot_frame> → map- Usually provided by SLAM
rsmNode
This node realizes transitions between the different states.Subscribed Topics
operationMode (rsm_msgs/OperationMode)- The current operation mode as set by the GUI or interrupts
- Navigation goal set in RViz
Published Topics
stateInfo (std_msgs/String)- Current state info text
Services
startStopExploration (std_srvs/SetBool)- Call to start or stop exploration, depending on the bool value (true: start, false: stop)
- Call to start or stop waypoint following, depending on the bool value (true: start, false: stop)
- Get current state info text
- Call to stop navigation started through RViz tool 2D Nav Goal
Parameters
~update_frequency (float, default: 20)- Update rate in Hz
- Sets the plugin's name for the state calculating the next goal
- Sets the plugin's name for the navigation state
- Sets the plugin's name for the mapping state.
robotControlMuxNode
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)- Publishes teleoperation command velocities
- Publishes autonomy command velocities
- Publishes joystick commands
Published Topics
<cmd_vel_topic> (std_msgs/String)- Command velocity that the robot should follow
- The current operation mode as set by the GUI or interrupts
Services
setOperationMode (rsm_msgs/SetOperationMode)- Sets the operation mode to the given parameter
Parameters
~update_frequency (float, default: 20)- Update rate in Hz
- Topic name for the autonomy command velocity
- Topic name for the teleoperation command velocity
- Topic name for the command velocity the robot should follow
- Is a joystick used to command the robot
- Topic name for joystick commands
- Time until teleoperation is stopped when no new command is received
serviceProviderNode
This node provides services for saving and receiving data needed by the volatile states.Published Topics
waypoints (rsm_msgs/WaypointArray)- List of all waypoints and their information
- The current exploration mode (true: interrupt, false: finish)
- Information if the current goal is still viable (only active is exploration mode is set to "interrupt")
- Information if the robot is currently moving in reverse (true: reverse, false: forward)
Services
addWaypoint (rsm_msgs/AddWaypoint)- Add a waypoint to the list of waypoints
- Get list of waypoints
- Move the waypoint at the given position in the waypoint list
- Remove the waypoint at the given position in the waypoint list
- Set the waypoint at the given position in the waypoint list to visited
- Set the waypoint at the given position in the waypoint list to unreachable
- Reset all waypoint's status to default
- Sets the waypoint following mode (0: single, 1: roundtrip, 2: patrol)
- Sets the routine of the waypoint at the given position in the waypoint list
- Return the list of all waypoint routines available
- Sets the current navigation goal
- Gets the current navigation goal
- Handles exploration and waypoint following when a goal was completed
- Return the current robot pose in relation to the map frame
- Set the exploration mode (true: interrupt, false: finish)
- Get the exploration mode
- Set the robot to reverse mode (true: reverse, false: forward)
- Return the reverse mode (true: reverse, false: forward)
Parameters
~update_frequency (float, default: 20)- Update rate in Hz
- Transform frame for the robot
- Vector with waypoint routines available as state plugins
Required tf Transforms
<robot_frame> → map- Usually provided by SLAM
