These packages contain a solution for Teaching & Repeating Missions via teleoperations or autonomous control. This solution is built on top of ROS 2 and offers fully compliant task servers, behavior tree nodes, APIs, and tools with the Nav2 ecosystem for easy integration, evaulation, and deployment. This is completely independent of localization or sensing modalities, so it can work sensor-free or via localization from Vision, GPS, SLAM, or any other method. The implementation provides a Teach Mission server which records a robot's route (including velocities) in a provided global frame and a topic for robot actions to record for later replay. This can be anything from taking an image, lowering an implement, engaging a tool, toggling a light, etc. We also provide a Repeat Missions server which replays the recorded route and actions. This server can replay not just a single mission but a series of concatenated missions for longer composed routes in order (assuming their end-and-start poses are sufficiently close).
To facilitate the most reliable and accuracy relay, we provide by default design a dense mission recording model. This will record mission routes at a regular configurable frequency or distance rather than at occasional keypoints, to allow for highly reliable replay without free-space planning between waypoints. These missions may be directly sent to a controller algorithm for optimal path tracking characteristics for your application, including the use of the Speed Limit feature of Nav2's Controller Server to dynamically adjust the robot's speed based on the recorded mission data to roughly mimic the behavior if desired, else it may repeat the mission at a chosen consistent controller speed. If sparse keypoint recording is desired (such as to allow deviations from the recorded route), that may be accomplished configuring the recording frequency or distance to a lower value and designing the application accordingly. See the "Sparse vs Dense Recording" section below for more details.
This work was sponsored by Rotate8 and created by Open Navigation LLC.
Rotate8 provides cost-effective, modular, and repairable farmer-first robotics for a healthier world. Rotate8's goal is to simplify agricultural automation, creating intuitive, cost-effective technology that empowers all farmers.
This is split into a few packages:
opennav_teach_and_repeat_msgs: Message definitions for Teach and Repeat missions.opennav_teach_and_repeat: Task servers for Teach and Repeat missions.opennav_teach_and_repeat_rviz_plugins: Rviz plugins for visualizing and interacting with Teach and Repeat missions.opennav_teach_and_repeat_bt: Behavior Tree nodes and XMLs for Teach and Repeat missions.opennav_teach_and_repeat_navigator: Nav2 navigator plugin for Teach and Repeat missions.opennav_teach_and_repeat_demo: A demonstration package & scripts for Teach and Repeat missions.
Teaching A Mission
Missions are recorded by commanding the robot to move via teleoperation or autonomous control via autonomy that you would like to precisely replay.
As the robot moves, its pose in a global frame (e.g., map, odom, world) is recorded at a regular interval or distance to create a dense route using TF2.
Additionally, the server subscribes to a MissionOperation topic that contains information about robot actions performed along the route, such as taking pictures, activating tools, or other custom actions specific to the application.
These are stored alongside the recorded route with their corresponding timestamps, poses, and optionally velocities to replay them accurately during mission execution.
The recording is started via an action server goal request and stopped by a cancelation of the action server goal.
The mission may also be concluded after a fixed duration set in the action request.
The saved missions are saved in the missions file directory for later retrieval and replay by the requested mission's label.
Optionally, the recorded path can be smoothed before saving to reduce localization noise and jitter using the enable_path_smoothing parameter.
It is intended that this server is called via a higher-level application such as a user interface for teleoperation or product feature. It is not intended to be called directly from a behavior tree or Nav2 system (though it could be if desired).
Missions can be easily taught using the Rviz2 panel provided in opennav_teach_and_repeat_rviz_plugins as shown below:
Repeating Missions
The intended workflow for autonomous navigation is to use the opennav_teach_and_repeat_navigator's NavigateTaughtMissions behavior tree navigator to follow taught missions with the flexibility of autonomy design with Nav2's famed behavior trees.
Optionally, the RepeatMissions action server may be called directly to repeat taught missions without the behavior tree layer if composed at a different layer of the autonomy system.
In the preferred approach, the NavigateTaughtMissions behavior tree navigator plugin exposes an action server for a repeating taught missions action, which contains the missions to replay and behavior tree to use.
This information is passed into the behavior tree blackboard for use to create the autonomy behavior.
The behavior tree is then executed and would leverage the RepeatMissions action server node to perform the mission replay (alongside controller server, fallback behaviors, etc).
In the RepeatMissions action server, the provided missions are concatenated together and provided to the behavior tree in-order -- with validation for transition between taught missions to ensure the end of one mission is sufficiently close to the start of the next mission.
The route is then returned for the behavior tree to use as needed.
The action server continues to run in the background to provide the route, feedback on progress, republish any MissionOperations at the appropriate times while the mission progresses, and dynamically adjust the robot's speed limits according to the recorded mission data.
Note that if the mission involves pauses, the Time recording metric must be used to identify these pauses properly as well as publish any stop-related operations at the right time. Else, the Distance recording metric can be used to virtually ignore pauses (but may still slow down at them if velocity replay is enabled).
This approach allows the application-system or user interface to request missions based on mission label name or filepaths to make it as easy to use as possible. This can be called easily from the same Rviz2 panel for engineering testing and evaluation with a single mission.
Actions:
| Name | Type | Description |
|---|---|---|
teach_and_repeat/teach |
opennav_teach_and_repeat_msgs/action/TeachMission |
Action server to teach a mission by recording the robot's path |
Topics:
| Name | Type | Description |
|---|---|---|
teach_and_repeat/mission_markers |
nav_msgs/msg/Path |
Publishes the path being recorded for visualization |
mission_operations (configurable) |
opennav_teach_and_repeat_msgs/msg/MissionOperation |
Subscribes to mission operations to record along the path |
Actions:
| Name | Type | Description |
|---|---|---|
teach_and_repeat/repeat |
opennav_teach_and_repeat_msgs/action/RepeatMissions |
Action server to repeat taught missions |
Services:
| Name | Type | Description |
|---|---|---|
teach_and_repeat/get_mission_files |
opennav_teach_and_repeat_msgs/srv/GetMissionFiles |
Service to retrieve list of available mission files from the mission directory |
Topics:
| Name | Type | Description |
|---|---|---|
teach_and_repeat/taught_paths |
nav_msgs/msg/Path |
Publishes the loaded mission path for visualization |
mission_operations (configurable) |
opennav_teach_and_repeat_msgs/msg/MissionOperation |
Republishes mission operations at the appropriate times during replay |
Actions:
| Name | Type | Description |
|---|---|---|
navigate_taught_missions |
opennav_teach_and_repeat_msgs/action/NavigateTaughtMissions |
Action server to navigate taught missions using a behavior tree |
| Parameter | Type | Default | Description |
|---|---|---|---|
global_frame |
string | "map" |
Global reference frame for transformations |
robot_base_frame |
string | "base_link" |
Robot base frame for transforms |
recording_interval |
double | 0.1 |
Recording interval in seconds (for TIME metric) or meters (for DISTANCE metric) |
transform_tolerance |
double | 0.1 |
Tolerance for TF transforms in seconds |
recording_metric |
string | "distance" |
Metric for recording: "time" or "distance" |
operation_topic |
string | "mission_operations" |
Topic name for mission operations to record |
mission_directory |
string | Required | Directory where missions are stored (must exist) |
append_timestamp_to_filename |
bool | false |
Whether to append timestamp to mission filename |
enable_path_smoothing |
bool | true |
Enable path smoothing to reduce localization noise before saving |
control_point_spacing |
int | 5 |
B-spline control point spacing. Higher values = more smoothing (typical: 3-10) |
odom_topic |
string | "odom" |
Odometry topic for velocity smoothing |
odom_smoother_duration |
double | 0.3 |
Duration in seconds for odometry velocity smoothing |
| Parameter | Type | Default | Description |
|---|---|---|---|
global_frame |
string | "map" |
Global reference frame for transformations |
robot_base_frame |
string | "base_link" |
Robot base frame for transforms |
operation_topic |
string | "mission_operations" |
Topic name for mission operations for replay |
speed_limit_topic |
string | "speed_limit" |
Topic name for publishing velocity/speed limit from taught mission |
mission_directory |
string | Required | Directory where missions are stored (must exist) |
via_point_tolerance |
double | 0.5 |
Tolerance to consider a via point reached (meters) |
max_path_deviation_warning |
double | 2.0 |
Maximum path tracking deviation from path before warning (meters) |
transform_tolerance |
double | 0.1 |
Tolerance for TF transforms in seconds |
mission_completion_tolerance |
double | 1.0 |
Tolerance for mission completion (meters) |
velocity_update_period |
double | 3.0 |
Period in seconds for publishing velocity updates to speed_limit_topic. Set to negative value to disable velocity publishing entirely. |
initial_velocity_lookahead_distance |
double | 2.0 |
Distance in meters to look ahead for velocity at mission start. At the start of each mission, uses the velocity from the via point at this distance ahead until reached, then uses closest point velocity. Set to 0 or negative to disable. |
minimum_velocity_deadband |
double | 0.05 |
Minimum velocity magnitude (m/s) to consider valid for speed limit publishing. When lookahead lands in a stop zone (velocity below this threshold), the highest magnitude velocity between the current position and lookahead is used instead. |
odom_topic |
string | "odom" |
Odometry topic for velocity monitoring during stop detection |
enable_stop_detection |
bool | true |
Enable detection and replay of pauses/stops from time-recorded missions |
stop_velocity_threshold |
double | 0.05 |
Maximum velocity (m/s) to consider the robot stopped during recording |
min_stop_duration |
double | 2.0 |
Minimum duration (seconds) of a stop to be recognized as a stop region |
max_cluster_gap |
double | 0.5 |
Maximum time gap (seconds) between stopped points to cluster them into a single stop region |
mission_transition_pause |
double | 2.0 |
Pause duration (seconds) between concatenated missions. Set to 0 for immediate transition. |
Input Ports:
| Port | Type | Default | Description |
|---|---|---|---|
mission_label |
string | Required | Mission label for storage |
max_record_duration |
double | -1.0 |
Maximum recording duration in seconds (-1 for infinite) |
Output Ports:
| Port | Type | Description |
|---|---|---|
error_code |
uint16_t | Error code from the action |
error_msg |
string | Error message from the action |
recording_duration |
builtin_interfaces::msg::Duration | Recording duration |
mission_length |
double | Mission length in meters |
num_taught_operations |
int | Number of operations taught |
Input Ports:
| Port | Type | Default | Description |
|---|---|---|---|
mission_labels |
vector<string> | Required | Mission labels or paths to load (semicolon separated) |
max_trans_deviation_between_missions |
float | -1.0 |
Maximum translational deviation between mission transitions (-1 to disable) |
max_rot_deviation_between_missions |
float | 3.14 |
Maximum rotational deviation between mission transitions in radians (default allows any angle) |
enable_progress_tracking |
bool | true |
Enable progress tracking for replaying operations, velocity changes, and pauses/stops |
Output Ports:
| Port | Type | Description |
|---|---|---|
error_code |
uint16_t | Error code from the action |
error_msg |
string | Error message from the action |
repeat_duration |
builtin_interfaces::msg::Duration | Repeat action duration |
num_repeated_operations |
int | Number of operations repeated along the mission |
mission_path |
nav_msgs::msg::Path | Path comprising the mission(s) to repeat |
current_mission_path_idx |
int | Currently tracked pose index on the Path |
current_mission_idx |
int | Current mission being executed |
mission_complete |
bool | True when all missions have been completed |
A condition node that checks if the mission is complete.
Input Ports:
| Port | Type | Description |
|---|---|---|
mission_complete |
bool | Flag indicating if mission is complete |
Returns: SUCCESS when mission_complete is true, FAILURE otherwise.
A condition node that waits for a new valid path to become available on the blackboard. Tracks the last seen path timestamp to detect changes.
Input Ports:
| Port | Type | Description |
|---|---|---|
path |
nav_msgs::msg::Path | Path to check for validity |
Returns: SUCCESS when a new valid path is available (non-empty and different timestamp than last success), FAILURE otherwise.
Cancel nodes for aborting in-progress teach and repeat missions actions. Both inherit from BtCancelActionNode with no custom ports.
The package provides two behavior tree XML files for common use cases:
navigate_taught_missions_with_recoveries.xml
A behavior tree for navigating taught missions without velocity tracking, operations, or progress tracking. Uses enable_progress_tracking="False" so the RepeatMissions node completes immediately after loading the mission path. Suitable for missions without recorded operations where you want the path sent directly to the controller to move at a consistent speed. Includes recovery behaviors (costmap clearing, waiting, backup) but no freespace replanning.
navigate_taught_velocity_missions.xml
A behavior tree for navigating taught missions with velocity tracking and operations replay. Uses enable_progress_tracking="True" so the RepeatMissions node runs continuously to track progress, replay operations, and update velocity limits. Features:
IsWithinPathTrackingBoundsfor monitoring path deviationWaitForNewPathcondition to detect when new path segments are available (for multi-controller segment missions)IsMissionCompletecondition to determine when all missions are finished- Support for multiple controller segments within a single mission
- Recovery behaviors without freespace replanning
| Parameter | Type | Default | Description |
|---|---|---|---|
TaughtMissionsNavigator.missions_blackboard_id |
string | "mission_labels" |
Blackboard variable ID for mission labels |
TaughtMissionsNavigator.max_trans_deviation_between_missions_blackboard_id |
string | "max_trans_deviation_between_missions" |
Blackboard variable ID for max translational deviation between missions |
TaughtMissionsNavigator.max_rot_deviation_between_missions_blackboard_id |
string | "max_rot_deviation_between_missions" |
Blackboard variable ID for max rotational deviation between missions |
TaughtMissionsNavigator.max_deviation_from_mission_blackboard_id |
string | "max_deviation_from_mission" |
Blackboard variable ID for max allowed deviation from mission path |
TaughtMissionsNavigator.path_blackboard_id |
string | "mission_path" |
Blackboard variable ID for mission path |
default_teach_and_repeat_bt_xml |
string | opennav_teach_and_repeat_bt/... |
Default BehaviorTree XML file to use if none set in the action |
| Parameter | Type | Default | Description |
|---|---|---|---|
taught_missions_dir |
string | "/tmp" |
Directory containing taught missions for RViz panel |
The choice of sparse vs dense recording often depends on a designer's intent or the application requirements. There are some situations where it is more intuitive to record key waypoints, sparse points of interest, or occasional breadcrumbs along a route when 'general intent following' but not 'precise following' is desired. Largely, the way a designer approaches or thinks about the problem will dictate the recording density. Dense recording is often preferred for high-fidelity replay of a route, especially in environments where precision is critical or when it is following recorded routes with insufficient sensor coverage for dynamic replanning safely.
Sparse recording may be more suitable for applications where flexibility is needed, allowing the robot to adapt to dynamic environments or obstacles with freespace planning or larger freedom for MPC path tracking techniques like MPPI. If going this direction, you may need to use a path planner in your behavior tree rather than sending the recorded route directly to a controller. If the viapoints may be occupied, consider behavior tree logic to skip waypoints that are blocked, configure planners to have planning tolerances, or otherwise consider dynamic obstacle handling in the application design. Designers should consider the trade-offs between fidelity and flexibility when choosing the recording density.
However, it is generally sane to start with dense recording and then adjust as needed based on the application's performance and requirements. Dense recording provides a solid baseline for accurate route following, robots can still be configured to deviate from the route via mixed-planning or controller configurations, and may still be downsampled if needed.
When using RepeatMissions in a behavior tree XML, you can specify whether to enable progress tracking with the enable_progress_tracking port.
When false, this will return the mission path and other information but will not track progress (replay operations, velocity set, etc.).
This means the action is not long-running and will return relatively quickly after loading and validating the missions.
Thus it should act as a typical action server call in a behavior tree.
When set to true however, this will now be long-running for the duration of the task as it will be tracking progress along the taught mission(s), setting velocity limits, stopping/pausing from recorded stops, replaying operations at the appropriate times. The design of the behavior tree needs to then account for this long-running BT node as well as the FollowPath long-running BT node.