Show EOL distros: 

robot_statemachine: rsm_additions | rsm_core | rsm_msgs | rsm_rqt_plugins | rsm_rviz_plugins

Package Summary

The rsm_additions package includes plugins for the Robot Statemachine and exemplary launch files

robot_statemachine: rsm_additions | rsm_core | rsm_msgs | rsm_rqt_plugins | rsm_rviz_plugins

Package Summary

The rsm_additions package includes plugins for the Robot Statemachine and exemplary launch files

There are breaking changes for exploration goal handling between kinetic and melodic!

Additions to the RSM including all mandatory plugin states and a plugin routine state. Also an additional service provider is included

Documentation

This package implements plugins for the Calculate Goal State, the Navigation State and the Mapping State. Furthermore, the Reversing Routine State is an optional routine state plugin. The Additions Service Provider handles the data to be transferred between these plugins.

Calculate Goal State

The Calculate Goal State interfaces the ROS package explore_lite, subscribes to its visualization topic that shows frontiers on a 2D map and extracts the closest frontier center point to the robot as navigation goal. Therefore, it retrieves the robot's pose and calculates its distance to each of the frontier's center points. Also, previously failed goals are disregarded as potential navigation goals. If it fails to find a suitable goal for exploration, it returns an error message and transitions back to the Idle State

To have explore_lite running without directly sending commands to the navigation stack, a mock action server is constructed in the Additions Service Provider that leads the exploration to believe the goals are accepted. Otherwise it does not start to to calculate frontiers. Furthermore, explore_lite is launched with progress_timeout set to 3600 seconds which gives the robot ten hours to move, otherwise the exploration stops and needs to be relaunched.

The Navigation State realizes an interface to the navigation ROS package. It forwards received goals to the navigation stack and also gets feedback from it regarding the progress. If it fails, the goal is added to the failed goals list. If it succeeds, the failed goal list will be reset.

When standing still for too long, it transitions to the Idle State. Reaching the goal will initiate a transition to the Mapping State or the particular routine state if there is one available. If not, Waypoint Following State is called. After reaching a navigation goal provided by RViz and if waypoint following has ended, it transitions to Idle State.

Reverse driving is realised by running two navigation stacks, one for forward driving and one for reverse driving. This is explained in more detail later. For reverse driving the robot also features a transform to a reverse base frame. When driving in reverse, all output command velocities are negated by the Additions Service Provider. If the reverse mode is activated or deactivated, the goal is cancelled and sent to the reverse navigation.

Mapping State

There are two mapping plugins included. The first state is just a dummy state while the latter is swiveling a simulated kinect camera from left to right around a revoluting joint.

Mapping Dummy State

The Mapping Dummy State is just transitioning back to the Calculate Goal State as specific mapping procedures are only relevant for the particular robot.

Kinect Mapping State

Swivels a kinect camera mounted on a joint revoluting around the z-axis from left to right and back to its centered position to map the surrounding area. This only works for the implemented Gazebo simulation as it publishes commands to the joint the kinect is mounted on.

Reversing Routine State

A Routine State called Reversing Routine is also include and toggles the reverse mode when the routine is executed. This means the robot is driving in reverse when it was going forward before and vice versa.

Additions Service Provider

This data handler class retrieves the frontiers published by explore_lite for visualization, extracts each frontier's center and republishes them as possible exploration goals.

For driving in reverse mode, the velocity commands issued by the reverse navigation stack are also subscribed to and republished with negated linear velocities. It also provides a service that is called when reverse mode should be activated. Since nothing needs to be changed in the configuration to change to reverse mode, this service just replies that it was successful.

If the kinect mapping is interrupted, a service is provided that moves the camera back to its centered position while the RSM is continuing.

Reverse Robot Movement with the Navigation Stack

The following code needs to be included in your launch file (or the nodes launched respectively) to allow the robot to navigate in forward and reverse depending on the set mode:

Replace the dots with the usual parameters for the navigation stack. Both packages share the same parameters but for the robot base frame. To use a particular navigation stack, initiate it like in this tutorial and call the action for forward movement with "move_base" and for reverse movement with "move_base_reverse". }}}

Calculate Goal State

The Calculate Goal State interfaces the ROS package explore_lite, subscribes to its visualization topic that shows frontiers on a 2D map and extracts the closest frontier center point to the robot as navigation goal. Therefore, it retrieves the robot's pose and calculates its distance to each of the frontier's center points. Also, previously failed goals are disregarded as potential navigation goals. If it fails to find a suitable goal for exploration, it returns an error message and transitions back to the Idle State

To have explore_lite running without directly sending commands to the navigation stack, a mock action server is constructed in the Additions Service Provider that leads the exploration to believe the goals are accepted. Otherwise it does not start to to calculate frontiers. Furthermore, explore_lite is launched with progress_timeout set to 3600 seconds which gives the robot ten hours to move, otherwise the exploration stops and needs to be relaunched.

The Navigation State realizes an interface to the navigation ROS package. It forwards received goals to the navigation stack and also gets feedback from it regarding the progress. If it fails, the goal is added to the failed goals list. If it succeeds, the failed goal list will be reset.

When standing still for too long, it transitions to the Idle State. Reaching the goal will initiate a transition to the Mapping State or the particular routine state if there is one available. If not, Waypoint Following State is called. After reaching a navigation goal provided by RViz and if waypoint following has ended, it transitions to Idle State.

Reverse driving is realised by running two navigation stacks, one for forward driving and one for reverse driving. This is explained in more detail later. For reverse driving the robot also features a transform to a reverse base frame. When driving in reverse, all output command velocities are negated by the Additions Service Provider. If the reverse mode is activated or deactivated, the goal is cancelled and sent to the reverse navigation.

Furthermore, if the robot got stuck during navigation and the respective parameter was set, reverse driving can be used to try to unstuck the robot.

Mapping State

There are two mapping plugins included. The first state is just a dummy state while the latter is swiveling a simulated Intel RealSense camera from left to right around a revoluting joint.

Mapping Dummy State

The Mapping Dummy State is just transitioning back to the Calculate Goal State as specific mapping procedures are only relevant for the particular robot.

RealSense Mapping State

Swivels an Intel RealSense camera mounted on a joint revoluting around the z-axis from left to right and back to it's centered position to map the surrounding area. This only works for the implemented Gazebo simulation as it publishes commands to the joint the RealSense is mounted on.

Reversing Routine State

A Routine State called Reversing Routine is also include and toggles the reverse mode when the routine is executed. This means the robot is driving in reverse when it was going forward before and vice versa.

Additions Service Provider

This data handler class provides services for the calcualte goal, navigation, mapping and reversing routine states. The respective services are only initialised if the corresponding state is active.

It retrieves the frontiers published by explore_lite for visualization, extracts each frontier's center and republishes them as possible exploration goals.

For driving in reverse mode, the velocity commands issued by the reverse navigation stack are also subscribed to and republished with negated linear velocities. It also provides a service that is called when reverse mode should be activated. Since nothing needs to be changed in the configuration to change to reverse mode, this service just replies that it was successful.

If the kinect mapping is interrupted, a service is provided that moves the camera back to its centered position while the RSM is continuing.

Gazebo To TF

This class mocks the robot's localization in a simulated map by retrieving it directly from gazebo. This can be used to replace error prone odometry or SLAM if something else needs to be tested.

Requires to include the following code snippet in the robot's URDF to work (This publishes a ROS message with the robot's position under the topic "ground_truth"):

Reverse Robot Movement with the Navigation Stack

The following code needs to be included in your launch file (or the nodes launched respectively) to allow the robot to navigate in forward and reverse depending on the set mode:

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
        ...
        <param name="global_costmap/robot_base_frame" value="$(arg robot_frame)" />
        <param name="local_costmap/robot_base_frame" value="$(arg robot_frame)" />
        <remap from="/cmd_vel" to="$(arg autonomy_cmd_vel_topic)" />
    </node>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base_reverse" output="screen">
        ...
        <param name="global_costmap/robot_base_frame" value="$(arg robot_frame)_reverse" />
        <param name="local_costmap/robot_base_frame" value="$(arg robot_frame)_reverse" />
        <remap from="/cmd_vel" to="$(arg autonomy_cmd_vel_topic)_reverse" />
        <remap from="move_base/goal" to="move_base_reverse/goal" />
        <remap from="move_base/cancel" to="move_base_reverse/cancel" />
        <remap from="move_base/feedback" to="move_base_reverse/feedback" />
        <remap from="move_base/status" to="move_base_reverse/status" />
        <remap from="move_base/result" to="move_base_reverse/result" />
    </node>

    <node pkg="tf" type="static_transform_publisher" name="base_footprint_reverse" args="0 0 0 3.1415 0 0 (arg robot_frame) $(arg robot_frame)_reverse 10" />

Replace the dots with the usual parameters for the navigation stack. Both packages share the same parameters but for the robot base frame. To use a particular navigation stack, initiate it like in this tutorial and call the action for forward movement with "move_base" and for reverse movement with "move_base_reverse". }}}

Nodes

additionalServiceProviderNode

An additional data handler class that adds services to interface the exploration lite and navigation packages.

Subscribed Topics

<autonomy_cmd_vel_top>_reverse (std_msgs/String)
  • Topic name for the autonomy command velocity in reverse mode
explore/frontiers (visualization_msgs/MarkerArray)
  • All frontier grid cells as points and closest frontier points as spheres

Published Topics

<_autonomy_cmd_vel_topic> (std_msgs/String)
  • Topic name for the autonomy command velocity
explorationGoals (geometry_msgs/PoseArray)
  • List of all currently available exploration goals
kinect_controller/command (std_msgs/Float64)
  • Position the kinect revolute joint will move to

Services

setNavigationToReverse (std_srvs/SetBool)
  • Needs to be implemented for reverse mode, just returns success

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
~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.

additionalServiceProviderNode

An additional data handler class that adds services to interface the exploration lite and navigation packages.

Subscribed Topics

<autonomy_cmd_vel_top>_reverse (std_msgs/String)
  • Topic name for the autonomy command velocity in reverse mode
explore/frontiers (visualization_msgs/MarkerArray)
  • All frontier grid cells as points and closest frontier points as spheres
explorationMode (std_msgs/Bool)
  • The current exploration mode (true: interrupt, false: finish)
explorationGoalStatus (rsm_msgs/GoalStatus)
  • The currently active goal's status and pose

Published Topics

<_autonomy_cmd_vel_topic> (std_msgs/String)
  • Topic name for the autonomy command velocity
explorationGoals (geometry_msgs/PoseArray)
  • List of all currently available exploration goals
realsense_controller/command (std_msgs/Float64)
  • Position the RealSense revolute joint will move to
failedGoals (geometry_msgs/PoseArray)
  • List of all previously failed goals
goalObsolete (std_msgs/Bool)
  • Information if the current goal is still viable (only active is exploration mode is set to Interrupt)

Services

setNavigationToReverse (std_srvs/SetBool)
  • Needs to be implemented for reverse mode, just returns success
resetRealsensePosition (std_srvs/Trigger)
  • Moves RealSense camera back to centered position

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
~calculate_goal_plugin (string, default: "rsm::CalculateGoalPlugin")
  • Sets the plugin's name for the calculate goal state
~navigation_plugin (string, default: "rsm::NavigationPlugin")
  • Sets the plugin's name for the navigation state
~mapping_plugin (string, default: "rsm::MappingDummyPlugin")
  • Sets the plugin's name for the mapping state.
exploration_goal_tolerance (double, default: 0.05)
  • Distance in all directions in meters that the robot's current position can differ from an exploration goal to still count it as reached

gazeboToTf

Mocks robot localization in gazebo simulation by retrieving it directly from gazebo

Subscribed Topics

<odom_topic> (nav_msgs/Odometry)
  • Position of the robot in gazebo simulation

Parameters

~odom_topic (string, default: "/ground_truth/state")
  • Topic name of odometry provided by gazebo
~map_frame (string, default: "map")
  • Name of the map frame
~robot_frame (string, default: "robot_footprint")
  • Name of the robot frame

Provided tf Transforms

<map_frame><robot_frame>
  • Transform between robot and map frame

Wiki: rsm_additions (last edited 2020-11-25 10:57:51 by MarcoSteinbrink)