RSM Core

The Robot Statemachine’s core components will be explained first and it’s usage afterwards, including examples and tutorials for writing plugins, including them into the RSM and setting up a robot. Also, handling the GUI and starting a simulation with the RSM is explained.

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. The implemented states can be seen in the biref class diagram below. It only shows the most important aspects and is not complete!

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 it’s 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 it’s name that is necessary to display the current state in the GUI and needs to be set in the onSetup or onEntry method. Furthermore, this name contains information if the state is part of an autonomous behavior. More details will follow later

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 it’s 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.

An exemplary state diagram with plugin states being shown with a bold border can be seen below.

Tutorials

The following section displays some examples and tutorials on how to use the RSM, starting with 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 it’s controls are shown and a tutorial on writing and including your own plugin state is presented last.

Set up a robot for use with RSM

Setting up a robot for the basic RSM usage is fairly straightforward since it only requires setting up a robot motor controller interface that subscribes to command velocity messages of type geometry_msgs/Twist and generates actual motor commands from them.

A service provider to tell the Boot State that the boot is finished is also required. This should ideally check if all necessary systems on your robot are up and running. The service provider needs to offer a service of type std_srvs/SetBool under the name “rsm/bootUpFinished”. The following code snippet shows a rudimentary sample implementation in a node:

#include "ros/ros.h"
#include "std_srvs/SetBool.h"

bool boot_finished = false;

bool bootUpService(std_srvs::SetBool::Request &req,
		std_srvs::SetBool::Response &res) {
	if (boot_finished) {
		res.success = 1;
		res.message = "Finished";
	} else {
		res.success = false;
		res.message = "Still booting ...";
	}
	return true;
}

int main(int argc, char **argv) {
	ros::init(argc, argv, "bootUpNode");
	ros::NodeHandle nh("rsm");
	ros::ServiceServer bootup_service = nh.advertiseService("bootUpFinished",
			bootUpService);
	//checking boot process and setting boot_finished to true if finished
	ros::spin();
	return 0;
}

If this is not possible or necessary for your configuration, you can just launch the bootUpNode from the RSM additions package that sets up the service provider and returns a successful boot message after default 1 second. The delay can be set using the parameter wait_time.

The setup for navigating to set goals and executing mapping behaviors or routines depends on the defined plugins and can therefore not generally be declared.

Note: If you plan on using the plugins for ROS navigation provided in the RSM additions package, you need to follow the navigation stack robot setup tutorial.

In general, a tool for navigation, a tool for mapping and a tool for exploration are necessary to fully exploit the robot RSM. This includes at least one sensor for sensing the environment, creating a 2D or 3D map and running SLAM. When provided with the particular exploration and navigation plugins, the RSM will work for 3D maps.

Run RSM

The RSM’s core functionality is distributed over several nodes that can simply be started with the launchfile rsm.launch which requires the following arguments:

• update_frequency: The update rate in Hz of the RSM (default: 20)
• robot_frame: The robot base frame (default: “base_footprint”)
• mapping_plugin: The plugin used for mapping (default: “rsm::MappingDummyState”)
• calculate_goal_plugin: The plugin used to calculate the next goal for exploration (default: “rsm::CalculateGoalState”)
• navigation_plugin: The plugin used for navigation (default: “rsm::NavigationState”)
• autonomy_cmd_vel_topic: The name of the command velocity topic for messages from exploration, waypoint following or simple goals (default: “/autonomy/cmd_vel”)
• teleoperation_cmd_vel_topic: The name of the command velocity topic for messages from teleoperation (default: “/teleoperation/cmd_vel”)
• cmd_vel_topic: The name of the command velocity topic that the motor controller interface subscribes to (default: “/cmd_vel)
• joystick_used: Is a joystick used for control and should autonomy be stopped when it is handled (default: “false”)
• joystick_topic: The name of the joystick topic for messages from handling the joystick (default: “/joy”)
• teleoperation_idle_timer: Time in seconds without input from teleoperation that leads to a transition to Idle State (default: 0.5)
• waypoint_routines: List of all plugins to be used as routines for waypoints (default: [])

Note: The default plugins mentioned above all exist in the RSM additions package.

The nodes can of course be started separately though it is easier to use the launch file.

Launch simulation

To demonstrate the RSM and get used to it’s controls, the RSM additions package offers three launch files that start a simulation including a complete robot and environment to start right away.

The first two simulations use the 3D Gazebo simulator which has to be installed before (If you have installed ROS with ros-melodic-desktop-full, it is already included).

If you want to simulate a TurtleBot3 Burger, your system requires the following two meta-packages: turtlebot3 and turtlebot3_simulations. The Clearpath Robotics Husky simulation depends on the husky simulator package which includes the robot to be simulated. Both TurtleBot3 and Husky had their URDFs customized to add a front-facing simulated RealSense depth camera. For the Husky it comes on an arch with a swivelling joint as a mount. Therefore, when running the exploration, the Mapping State actually moves the camera around to increase the perceived area.

The third simulation depends on the stdr simulator package which is solely in 2D and offers a much less CPU-intensive alternative to Gazebo. If your machine is not very powerful or you just want to have a quick peek at what the RSM has to offer, stick with the stdr simulator. Screenshots from two simulations can be seen below, Gazebo first and stdr simulator last, the simulation on the left and RViz on the right.

All simulations use the plugins implemented in RSM additions which need the following packages to be installed:

• gmapping for SLAM
• ROS navigation stack for the Navigation State
• explore lite for the Calculate Goal State

Follow these steps for installation (this tutorial assumes you are using catkin_tools, if you don’t, replace catkin build ... with catkin make):

sudo apt install ros-melodic-gmapping ros-melodic-navigation
cd /path/to/your/catkin_ws/src
git clone https://github.com/hrnr/m-explore.git
catkin build explore_lite

To install all dependencies for the TurtleBot3 Burger simulation, run these commands in a terminal:

sudo apt install ros-melodic-turtlebot3
sudo apt install ros-melodic-turtlebot3-simulations

To install the Husky simulation, run the following command in a terminal:

sudo apt install ros-melodic-husky-simulator

For the stdr simulator, follow these instructions (without qt4 the build will fail):

sudo apt install qt4-default
cd /path/to/your/catkin_ws/src
git clone https://github.com/stdr-simulator-ros-pkg/stdr_simulator.git
catkin build stdr_simulator

When the above prerequisites are met, the simulations can be launched with the following commands including a pre-configured RViz display. If you do not want to start RViz, just add rviz:=false.

For gazebo with the TurtleBot3 Burger robot:

roslaunch rsm_additions simulation_turtlebot3.launch

For gazebo with the Husky UGV:

roslaunch rsm_additions simulation_husky.launch

For the stdr simulator:

roslaunch rsm_additions simulation_stdr.launch

GUI introduction

The RSM can be operated through a GUI that enables the use of all it’s core functionalities. The GUI panel is depicted below and can be integrated into RViz or rqt. To the former by adding a new panel through Panels->Add New Panel and then choose RSMControlPanel under rsm_rviz_plugins. To the latter by adding a new plugin through Plugins->RSM Control. The GUI always shows which state is currently active and provides the options explained below.

The GUI offers control over the class handling the command velocities forwarded to the motor controller interface. This includes the software emergency stop as well as choosing autonomy, teleoperation or stopped. When the software emergency stop is active, the other choices are disabled and the command velocity is set to stopped until the software emergency stop is released again.

The exploration can be started and stopped by using the respective buttons in the GUI. Next to the button is a drop-down box where the exploration mode can be set. This mode can either be Finish or Interrupt where the former lets the robot reach each goal before transitioning to Mapping State while the latter starts the transition as soon as the current goal is no longer listed as an exploration goal. The mode can only be set before starting exploration and not while it is running.

Waypoint following can also be started and stopped through the respective buttons. Furthermore, when waypoint following is stopped, there is the option to reset the current progress and restore all waypoints to their initial values. It is possible to set the waypoint following mode using the drop-down box next to the formerly mentioned buttons. The waypoint following mode can be one of the following three and only changed when stopped:

• single
• roundtrip
• patrol

The single mode lets the robot start from the first waypoint and then to all consecutive ones. Upon reaching the last one it stops. In roundtrip mode, after reaching the last waypoint all waypoints are reset and it starts anew from waypoint one. This is repeated until manually stopped. Patrol mode works in a similar fashion. After reaching the last waypoint all waypoints are reset and it starts again in reverse order. The first and last waypoints are only targeted once and their attached routines executed only once as well. Also, it can only be stopped manually.

The GUI also offers the possibility to set a waypoint at the robot’s current location and with the robot’s current orientation. These waypoint’s routines can be set from the drop-down box next to the button setting the actual waypoint.

Furthermore, a checkbox enables setting the reverse mode manually. When the box is checked the robot moves in reverse. A button next to the checkbox enables stopping the navigation when a simple navigation goal was set through RViz.

When using RViz, waypoints can be set by utilizing the Plant Waypoint Tool (Hotkey: “w”). It can be added through the plus button (Add a new tool) in the toolbar and then choosing PlantWaypointTool under rsm_rviz_plugins. This enables putting waypoints on the ground plane, determining their x- and y-coordinates, and orientate them in yaw by dragging the mouse in the desired direction. They are depicted as interactive markers with a flagpole mesh and the number of the waypoint above. Accordingly, an interactive marker display needs to be added with the topic name waypoint_markers/update to show them. The color of the marker corresponds to the waypoint’s status: blue is the default color, green means the waypoint has been visited and red that it is unreachable.

The displayed markers are interactive and can be seen below. Using the circle around them, they can be dragged in the desired direction, changing their x-y-position and yaw-orientation. The arrows above and below can be used to drag them in the respective direction, altering it’s z-coordinate. Clicking on the waypoint marker opens a menu that offers the options to set the routine to be executed when reaching the waypoint and to delete the waypoint. The routine can also be set to none.

Note: When the robot is moving towards a waypoint, the specific waypoint can be manipulated but these changes will not be forwarded to the current navigation. So, changes made after the robot started to move towards the waypoint, will not be regarded until the waypoint and it’s possible routine was finished.

The RSM additions package features some exemplary RViz configuration files for the respective launch files that automatically include the GUI and Plant Waypoint Tool as well as adding the waypoint interactive marker topic to the display.

Note: When saving the RViz configuration, the Plant Waypoint Tool sometimes does not get included in the configuration and has to be added each time RViz is started manually. To fix this, you need to add - Class: rsm::PlantWaypointTool to your RViz configuration file by hand. It has to be appended under Visualization Manager: Tools as can be seen in the snippet below.

...
Visualization Manager:
  Class: ""
  Displays:
  ...
  Name: root
  Tools:
    ...
    - Class: rsm::PlantWaypointTool 
  Value: true
  ...

If the 2D Nav Goal Tool (Hotkey: “g”) from RViz should be used, the respective topic from RViz needs to be remapped, so that it works with the RSM. The following remap needs to be added to the RViz launch, otherwise the 2D Nav Goal Tool cannot be used:

<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d ...">
	<remap from="/move_base_simple/goal" to="/rsm/simpleGoal" />
</node>

Note: Replace the dots with the path to your configuration file.

Writing a plugin state

To create a plugin state to be used with the robot RSM follow the upcoming steps. This is very similar to the ROS tutorial Writing and Using a Simple Plugin but also includes some specific details for the RSM.

In your package, add the following code to the respective files:

CMakeLists.txt:

find_package(catkin REQUIRED COMPONENTS
  roscpp
  pluginlib
  rsm_core
  rsm_msgs
  ...
)

package.xml:

...
<build_depend>pluginlib</build_depend>
<build_export_depend>pluginlib</build_export_depend>
<exec_depend>pluginlib</exec_depend>
<exec_depend>rsm_core</exec_depend>
<build_depend>rsm_core</build_depend>
<build_export_depend>rsm_core</build_export_depend>
<build_depend>rsm_msgs</build_depend>
<build_export_depend>rsm_msgs</build_export_depend>
<exec_depend>rsm_msgs</exec_depend>
...

This adds all dependencies needed to use the pluginlib and include the Base State.
Next, create a class consisting of a header and source file in the respective directory in your package. The class needs to inherit from the Base State, interact with the State Interface and declare it is a plugin. If the plugin is part of exploration or waypoint following, its name must start with a leading "E:" or "W:" respectively. This is used to check, if the particular behavior is still active. The sample code for header and source are shown below.

ExampleState.h:

#include <pluginlib/class_list_macros.h>
#include <rsm_core/BaseState.h>
#include <rsm_core/StateInterface.h>

namespace rsm {

class ExampleState: public BaseState {

public:
	ExampleState();
	~ExampleState();
	void onSetup();
	void onEntry();
	void onActive();
	void onExit();
	void onExplorationStart(bool &success, std::string &message);
	void onExplorationStop(bool &success, std::string &message);
	void onWaypointFollowingStart(bool &success, std::string &message);
	void onWaypointFollowingStop(bool &success, std::string &message);
	void onInterrupt(int interrupt);
};

}

ExampleState.cpp:

#include "ExampleState.h"

namespace rsm {

ExampleState::ExampleState() {
	//...
}

ExampleState::~ExampleState() {
	//...
}

void ExampleState::onSetup() {
	//...
}

void ExampleState::onEntry() {
	//...
}

void ExampleState::onActive() {
	//...
}

void ExampleState::onExit() {
	//...
}

void ExampleState::onExplorationStart(bool &success,
		std::string &message) {
	//...
}

void ExampleState::onExplorationStop(bool &success,
		std::string &message) {
	//...
}

void ExampleState::onWaypointFollowingStart(bool &success,
		std::string &message) {
	//...
}

void ExampleState::onWaypointFollowingStop(bool &success,
		std::string &message) {
	//...
}

void ExampleState::onInterrupt(int interrupt) {
	//...
}

}

PLUGINLIB_EXPORT_CLASS(rsm::ExampleState,
		rsm::BaseState)

The state plugin needs to implement all methods declared in the Base State as virtual and enables to add arbitrary functionality to them. The PLUGINLIB_EXPORT_CLASS macro registers the class as a plugin to the pluginlib.

To make the plugin available to ROS, an XML file needs to be added in the package that declares them as a library. The file should look like this:
rsm_example_plugins.xml:

<library path="lib/librsm_example_plugins">
	<class type="rsm::ExampleState"
		base_class_type="rsm::BaseState">
		<description>This is the example state.</description>
	</class>
	...
</library>

It can feature multiple classes to declare in the same manner.
The plugin library needs to be exported as well. Therefore the following lines need to be added to the package.xml:

<export>
	<rsm plugin="${prefix}/rsm_example_plugins.xml" />
</export>

Note: There can only be one export bracket in each package.xml.

With the following statement you can check in the terminal if the plugin was registered correctly:

rospack plugins --attrib=plugin rsm

It should show:

"your_package_name" /"your_workspace_path"/src/"your_package_name"/rsm_example_plugins.xml
rsm_additions /home/marco/catkin_ws/src/robot_rsm/rsm_additions/rsm_plugins.xml

You are now able to use the plugin state in the RSM.

Use plugin state in the RSM

To use the created plugin from above in the RSM, it has to be made known to the State Interface. This needs to be done by setting the respective parameters, either through the launch file or manually when starting the nodes from console. The State Interface expects the names for the Calculate Goal State, the Mapping State and the Navigation State plugins. The waypoint Routine State plugins need to be given to the Service Provider. A sample launch with set parameters can be seen in the snippet below, where the plugins defined in RSM additions are used. A detailed example can be seen in the RSM additions launch files.

<include file="$(find rsm_core)/launch/rsm.launch">
	<arg name="calculate_goal_plugin" value="rsm::CalculateGoalState" />
	<arg name="navigation_plugin" value="rsm::NavigationState" />
	<arg name="mapping_plugin" value="rsm::MappingState" />
	<arg name="waypoint_routines" value="['Reversing']" />
	...
</include>

The provided plugins can have arbitrary names, though it is recommended to use the rsm namespace to avoid collisions with other packages since the plugin names need to be unique.

For waypoint routines up to ten routines can be provided as an array. Each routine plugin must be named like this <pre>rsm::NameRoutineState</pre> Name has to be replaced by a uniquely identifying name for the particular routine. For the routines parameter provided only the Name needs to be set. So in the above example the plugin corresponding to the Reversing routine is called rsm::ReversingRoutineState.

If additional plugins should be used, their names do not need to be made known to the RSM up front. How to include them will be explained below.

For a transition to another state, in your implementation of a state the transitionToVolatileState method from State Interface needs to be called. If a transition to one of the included states is desired, it needs to be included in the header file and and then initialized and handed over as a parameter to the previously mentioned method. This is the first of the below examples. The second is a transition to a plugin state, which is made by providing one of the predefined types from State Interface to the method. If it is a routine plugin that should be called, the routine plugin’s name needs to be provided as well, see the third example. The last example shows a transition to an additional plugin, where a 0 for the plugin type needs to be given and the name of the plugin without a leading rsm:: prefix.

1. Transition to already included state:
   _stateinterface->transitionToVolatileState(boost::make_shared<IdleState>());
2. Transition to plugin state for navigation:
   _stateinterface->transitionToVolatileState(_stateinterface->getPluginState(NAVIGATION_STATE));
3. Transition to routine plugin state:
   _stateinterface->transitionToVolatileState(_stateinterface->getPluginState(ROUTINE_STATE, "Reversing"));
4. Transition to additional plugin state:
   _stateinterface->transitionToVolatileState(_stateinterface->getPluginState(0, "ClimbStairsState"));

For a reference implementation of the Calculate Goal State, the Navigation State, the Mapping State and a routine plugin called Reversing Routine State, see the rsm additions package.

If additional data has to be passed between plugin states, that is not already covered by the Service Provider, it is recommended to implement an additional data handler for this. See the Additions Service Provider in the package RSM addtions for an example.

If you want to create your own plugin for calculating an exploration goal or navigation, you should meet the following requirements to enable all of the RSM’s functionalities.

Calculate Goal Plugin

The calculate goal plugin is required to set a new navigation goal before transitioning to the navigation state. Therefore, it has to call the respective service as shown in the exemplary code below.

...
ros::NodeHandle nh("rsm");
ros::ServiceServer set_navigation_goal_service = nh.serviceClient<rsm_msgs::SetNavigationGoal>("setNavigationGoal");
...
rsm_msgs::SetNavigationGoal srv;
geometry_msgs::Pose goal;
// set goal to the new exploration target calculated by your algorithm
srv.request.goal = goal;
srv.request.navigationMode = EXPLORATION;
if (!set_navigation_goal_service.call(srv)) {
	ROS_ERROR("Failed to call Set Navigation Goal service");
}
...

Furthermore, if the Exploration is run in Interrupt mode, the additional service provider is required to publish a goalObsolete topic of type std_msgs/Bool, publishing the value “true” if the goal has become obsolete.

Navigation Plugin

The navigation plugin requires a larger number of services and topics that need to be processed to fully interface all of the RSM’s abilities.

First, to retrieve the navigation goal, the getNavigationGoal service has to be called. Furthermore, the getReverseMode service needs to be called as well. And in case the reverse mode changes, a callback for the reverseMode topic is also necessary. To properly interact with exploration, the getExplorationMode service must be called and if the mode is set to Interrupt, the goalObsolete topic must be subscribed to. In case the goal becomes obsolete, the navigation finishes.

When a goal is reached or cannot be reached, the navigation ends and the navigationGoalCompleted service must be called, providing information about the success of the navigation to it. Have a look at the Navigation plugin state in the rsm_additions package for details regarding the implementation.

If the robot should be able to move in reverse mode, a service needs to be implemented called setNavigationToReverse which changes the navigation’s mode interface in the Navigation State plugin and switches between forward and reverse movement. A sample to include into the additional data handler can be seen below. If it is missing, activating reverse mode will only output a matching error.

...
ros::NodeHandle nh("rsm");
ros::ServiceServer set_navigation_to_reverse_service = nh.advertiseService("setNavigationToReverse", setNavigationToReverse);
...

bool setNavigationToReverse(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) {
	if (//set mode to forward/reverse depending on req.data) {
		res.success = 1;
		res.message = "Mode set";
	} else {
		res.success = 0;
		res.message = "Mode not set";
	}
	return true;
}

Nodes

rsmNode

This node realizes transitions between the different states.

Note: All topics and services are in the rsm namespace.

Subscribed Topics

operationMode (rsm_msgs/OperationMode)
The current operation mode as set by the GUI or interrupts

simpleGoal (geometry_msgs/PoseStamped)
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)

startStopWaypointFollowing (std_srvs/SetBool)
Call to start or stop waypoint following, depending on the bool value (true: start, false: stop)

stateInfo (std_srvs/Trigger)
Get current state info text

stop2DNavGoal (std_srvs/Trigger) Call to stop navigation started through RViz tool 2D Nav Goal

Parameters

~update_frequency (float, default: 20)
Update rate in Hz

~calculate_goal_plugin (string, default: “rsm::CalculateGoalPlugin”)
Sets the plugin’s name for the state calculating the next goal.

~navigation_plugin (string, default: “rsm::NavigationPlugin”)
Sets the plugin’s name for the navigation state.

~mapping_plugin (string, default: “rsm::MappingPlugin”)
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.

Note: All topics and services are in the rsm namespace.

Subscribed Topics

**** ([std_msgs/String](http://docs.ros.org/api/std_msgs/html/msg/String.html)) Publishes teleoperation command velocities

**** ([std_msgs/String](http://docs.ros.org/api/std_msgs/html/msg/String.html)) Publishes autonomy command velocities

**** ([std_msgs/String](http://docs.ros.org/api/std_msgs/html/msg/String.html)) Publishes joystick commands

Published Topics

**** ([std_msgs/String](http://docs.ros.org/api/std_msgs/html/msg/String.html)) Command velocity that the robot should follow.

operationMode (rsm_msgs/OperationMode)
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

~autonomy_cmd_vel_topic (string, default: “autonomy/cmd_vel”)
Topic name for the autonomy command velocity

~teleoperation_cmd_vel_topic (string, default: “teleoperation/cmd_vel”)
Topic name for the teleoperation command velocity

~cmd_vel_topic (string, default: “cmd_vel”)
Topic name for the command velocity the robot should follow

~joystick_used (bool, default: “false”)
Is a joystick used to command the robot

~joystick_topic (string, default: “joy”)
Topic name for the joystick commands the robot is listening to

~teleoperation_idle_timer (double, default: 0.5)
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.

Note: All topics and services are in the rsm namespace.

Published Topics

waypoints (rsm_msgs/WaypointArray)
List of all waypoints and their information

explorationMode (std_msgs/Bool)
The current exploration mode (true: interrupt, false: finish)

explorationGoalStatus (rsm_msgs/GoalStatus)
The currently active goal’s status and pose

reverseMode (std_msgs/Bool)
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

getWaypoints (rsm_msgs/GetWaypoints)
Get list of waypoints

moveWaypoint (rsm_msgs/MoveWaypoint)
Move the waypoint at the given position in the waypoint list

removeWaypoint (rsm_msgs/RemoveWaypoint)
Remove the waypoint at the given position in the waypoint list

waypointVisited (rsm_msgs/WaypointVisited)
Set the waypoint at the given position in the waypoint list to visited

waypointUnreachable (rsm_msgs/WaypointUnreachable)
Set the waypoint at the given position in the waypoint list to unreachable

resetWaypoints (std_srvs/Trigger)
Reset all waypoint’s status to default

setWaypointFollowingMode (rsm_msgs/SetWaypointFollowingMode)
Sets the waypoint following mode (0: single, 1: roundtrip, 2: patrol)

setWaypointRoutine (rsm_msgs/SetWaypointRoutine)
Sets the routine of the waypoint at the given position in the waypoint list

getWaypointRoutines (rsm_msgs/GetWaypointRoutines)
Return the list of all waypoint routines available

setNavigationGoal (rsm_msgs/SetNavigationGoal)
Sets the current navigation goal

getNavigationGoal (rsm_msgs/GetNavigationGoal)
Gets the current navigation goal

NavigationGoalCompleted (std_srvs/Trigger)
Handles exploration and waypoint following when a goal was completed

getRobotPose (rsm_msgs/GetRobotPose)
Return the current robot pose in relation to the map frame

setExplorationMode (std_srvs/SetBool)
Set the exploration mode (true: interrupt, false: finish)

getExplorationMode (std_srvs/Trigger)
Get the exploration mode

SetReverseMode (std_srvs/SetBool)
Set the robot to reverse mode (true: reverse, false: forward)

GetReverseMode (std_srvs/Trigger)
Return the reverse mode (true: reverse, false: forward)

Parameters

~update_frequency (float, default: 20)
Update rate in Hz

~robot_frame (string, default: “/base_link”)
Transform frame for the robot

~waypoint_routines (std::vector, default: []) Vector with waypoint routines available as state plugins

Required tf Transforms

** -> map** Usually provided by SLAM

Changelog for package rsm_core

1.2.1 (2020-11-24)

1.2.0 (2020-11-04)

• Added parameter for navigation behavior on idle timer callback and gazeboToTf changed to ground plane
• Replaced simulated kinect with Intel RealSense to adapt to husky simulator melodic
• Now starts exploration with mapping state
• Exploration Goal Status is now published as a topic to prevent service deadlocks
• Removed failed goal services and added a publisher
• Added GoalCompleted service to give more detailed feedback over navigation completed status
• Added prefix to state name to identify autonomy behavior
• moved goalObsolete service to Additions and exploration completed goal out of navigation completed service
• Fix wrong error message on service failure
• Added class and state diagram to documentation
• Added tutorial details for creating Calculate Goal and Navigation plugins
• Added services for handling completed navigation goals to remove the logic from the navigation plugin
• Moved logic regarding explore_lite to AdditionsServiceProvider from ServiceProvider where they should belong
• Removed double maintainer tags
• Updated README with joystick listener addition
• Added joystick topic for teleoperation interrupt
• Properly reset shared_ptr to prevent errors on close
• Contributors: Marco Steinbrink, MarcoStb1993, marco

1.1.3 (2019-08-29)

• Added all dependencies to CMakeLists and package.xml, that were missing previously
• Contributors: MarcoStb1993

1.1.2 (2019-08-28)

1.1.1 (2019-08-05)

• added changelogs
• Fixed some dependencies for the new names
• Changed package names, this time for real
• Contributors: MarcoStb1993