Nav2 Demo (Hierarchical FSM + ROS 2 Action)

This tutorial demonstrates how to integrate YASMIN with ROS 2 Navigation (Nav2) using a hierarchical state machine in C++. Hierarchical FSMs allow you to nest state machines within states, creating modular and reusable navigation behaviors. This pattern is ideal for complex robot tasks where high-level planning (waypoint selection) is separated from low-level execution (navigation to each point).

⚠️ Prerequisites - Create a ROS 2 Package First:

Before following this tutorial, you need to create a new ROS 2 package to contain your Nav2 state machine code. The nav2_demo source files shown here are not included in the default yasmin_demos package.

Create your package with:

ros2 pkg create --build-type ament_cmake my_nav2_demo \
  --dependencies yasmin yasmin_ros nav2_msgs geometry_msgs

Then add the source code from this tutorial to your new package.

Overview

The demo creates a two-level hierarchical state machine. The parent state machine handles waypoint management: it creates a dictionary of named waypoint coordinates (entrance, bathroom, livingroom, kitchen, bedroom), randomly selects a subset for the mission, and delegates navigation to a child state machine. The child state machine implements the navigation loop: it retrieves the next waypoint from the list, converts coordinates to a Pose message, sends it to Nav2's NavigateToPose action server, and repeats until all waypoints are visited. This structure demonstrates state machine composition, shared blackboard data between levels, and ROS 2 action integration for real robot navigation.

The workflow consists of:

  1. Create Waypoints: Initialize a dictionary with 5 predefined locations (x, y, orientation_z, orientation_w).
  2. Select Random Subset: Choose N random waypoints from the dictionary based on waypoints_num blackboard parameter.
  3. Navigate (Hierarchical): Enter the nested navigation state machine that loops through selected waypoints, sending each to Nav2's action server.

Running the Demo

After creating your package and adding the source code, build and run: 

# Build your package
colcon build --packages-select my_nav2_demo
source install/setup.bash

# Run the demo
ros2 run my_nav2_demo nav2_demo

Note: Make sure you have Nav2 running with a valid map before executing this demo.

Source Code

You can find the full source code in the yasmin_demos package. 

#include <algorithm>
#include <iostream>
#include <map>
#include <memory>
#include <random>
#include <string>
#include <vector>

#include "geometry_msgs/msg/pose.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"

#include "yasmin/blackboard.hpp"
#include "yasmin/cb_state.hpp"
#include "yasmin/logs.hpp"
#include "yasmin/state_machine.hpp"
#include "yasmin_ros/action_state.hpp"
#include "yasmin_ros/basic_outcomes.hpp"
#include "yasmin_ros/ros_logs.hpp"
#include "yasmin_viewer/yasmin_viewer_pub.hpp"

using std::placeholders::_1;
using std::placeholders::_2;
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using Pose = geometry_msgs::msg::Pose;

// Constants for state outcomes
const std::string HAS_NEXT = "has_next"; ///< Indicates there are more waypoints
const std::string END = "end";           ///< Indicates no more waypoints

class Nav2State : public yasmin_ros::ActionState<NavigateToPose> {
public:
  Nav2State()
      : yasmin_ros::ActionState<NavigateToPose>(
            "/navigate_to_pose",
            std::bind(&Nav2State::create_goal_handler, this, _1)) {}

  NavigateToPose::Goal create_goal_handler(
      yasmin::Blackboard::SharedPtr blackboard) {
    NavigateToPose::Goal goal;
    goal.pose.pose = blackboard->get<Pose>("pose");
    goal.pose.header.frame_id = "map"; // Set the reference frame to 'map'
    return goal;
  }
};

std::string
create_waypoints(yasmin::Blackboard::SharedPtr blackboard) {
  std::map<std::string, std::vector<double>> waypoints = {
      {"entrance", {1.25, 6.30, -0.78, 0.67}},
      {"bathroom", {4.89, 1.64, 0.0, 1.0}},
      {"livingroom", {1.55, 4.03, -0.69, 0.72}},
      {"kitchen", {3.79, 6.77, 0.99, 0.12}},
      {"bedroom", {7.50, 4.89, 0.76, 0.65}}};
  blackboard->set<std::map<std::string, std::vector<double>>>("waypoints",
                                                              waypoints);
  return yasmin_ros::basic_outcomes::SUCCEED;
}

std::string take_random_waypoint(
    yasmin::Blackboard::SharedPtr blackboard) {
  auto waypoints =
      blackboard->get<std::map<std::string, std::vector<double>>>("waypoints");
  int waypoints_num = blackboard->get<int>("waypoints_num");

  std::vector<std::string> waypoint_names;
  for (const auto &pair : waypoints) {
    waypoint_names.push_back(pair.first);
  }

  std::random_device rd;
  std::mt19937 g(rd());
  std::shuffle(waypoint_names.begin(), waypoint_names.end(), g);
  std::vector<std::string> random_waypoints(
      waypoint_names.begin(), waypoint_names.begin() + waypoints_num);

  blackboard->set<std::vector<std::string>>("random_waypoints",
                                            random_waypoints);
  return yasmin_ros::basic_outcomes::SUCCEED;
}

std::string
get_next_waypoint(yasmin::Blackboard::SharedPtr blackboard) {
  auto random_waypoints =
      blackboard->get<std::vector<std::string>>("random_waypoints");
  auto waypoints =
      blackboard->get<std::map<std::string, std::vector<double>>>("waypoints");

  if (random_waypoints.empty()) {
    return END;
  }

  std::string wp_name = random_waypoints.back();
  random_waypoints.pop_back();
  blackboard->set<std::vector<std::string>>("random_waypoints",
                                            random_waypoints);

  auto wp = waypoints.at(wp_name);

  Pose pose;
  pose.position.x = wp[0];
  pose.position.y = wp[1];
  pose.orientation.z = wp[2];
  pose.orientation.w = wp[3];

  blackboard->set<Pose>("pose", pose);
  blackboard->set<std::string>("text", "I have reached waypoint " + wp_name);

  return HAS_NEXT;
}

int main(int argc, char *argv[]) {
  YASMIN_LOG_INFO("yasmin_nav2_demo");
  rclcpp::init(argc, argv);
  yasmin_ros::set_ros_loggers();

  auto sm = yasmin::StateMachine::make_shared(
      std::initializer_list<std::string>{yasmin_ros::basic_outcomes::SUCCEED,
                                         yasmin_ros::basic_outcomes::ABORT,
                                         yasmin_ros::basic_outcomes::CANCEL},
      true);
  auto nav_sm = yasmin::StateMachine::make_shared(
      std::initializer_list<std::string>{yasmin_ros::basic_outcomes::SUCCEED,
                                         yasmin_ros::basic_outcomes::ABORT,
                                         yasmin_ros::basic_outcomes::CANCEL});

  sm->add_state(
      "CREATING_WAYPOINTS",
      yasmin::CbState::make_shared(
          std::initializer_list<std::string>{
              yasmin_ros::basic_outcomes::SUCCEED},
          create_waypoints),
      std::map<std::string, std::string>{
          {yasmin_ros::basic_outcomes::SUCCEED, "TAKING_RANDOM_WAYPOINTS"}});
  sm->add_state("TAKING_RANDOM_WAYPOINTS",
                yasmin::CbState::make_shared(
                    std::initializer_list<std::string>{
                        yasmin_ros::basic_outcomes::SUCCEED},
                    take_random_waypoint),
                std::map<std::string, std::string>{
                    {yasmin_ros::basic_outcomes::SUCCEED, "NAVIGATING"}});

  nav_sm->add_state(
      "GETTING_NEXT_WAYPOINT",
      yasmin::CbState::make_shared(
          std::initializer_list<std::string>{END, HAS_NEXT}, get_next_waypoint),
      std::map<std::string, std::string>{
          {END, yasmin_ros::basic_outcomes::SUCCEED},
          {HAS_NEXT, "NAVIGATING"}});
  nav_sm->add_state(
      "NAVIGATING", std::make_shared<Nav2State>(),
      std::map<std::string, std::string>{
          {yasmin_ros::basic_outcomes::SUCCEED, "GETTING_NEXT_WAYPOINT"},
          {yasmin_ros::basic_outcomes::CANCEL,
           yasmin_ros::basic_outcomes::CANCEL},
          {yasmin_ros::basic_outcomes::ABORT,
           yasmin_ros::basic_outcomes::ABORT}});

  sm->add_state(
      "NAVIGATING", nav_sm,
      std::map<std::string, std::string>{{yasmin_ros::basic_outcomes::SUCCEED,
                                          yasmin_ros::basic_outcomes::SUCCEED},
                                         {yasmin_ros::basic_outcomes::CANCEL,
                                          yasmin_ros::basic_outcomes::CANCEL},
                                         {yasmin_ros::basic_outcomes::ABORT,
                                          yasmin_ros::basic_outcomes::ABORT}});

  yasmin_viewer::YasminViewerPub yasmin_pub(sm, "YASMIN_NAV2_DEMO");

  auto blackboard = yasmin::Blackboard::make_shared();
  blackboard->set<int>("waypoints_num", 2);

  try {
    std::string outcome = (*sm.get())(blackboard);
    YASMIN_LOG_INFO(outcome.c_str());
  } catch (const std::exception &e) {
    YASMIN_LOG_WARN(e.what());
  }

  rclcpp::shutdown();

  return 0;
}