ROS 2 State Machines & Behavior Trees Guide 2026

Design complex robot behaviors with state machines and behavior trees. Coordinate multi-step tasks, implement hierarchical control, and achieve autonomous decision-making in ROS 2.

1. Finite State Machines (FSM) in ROS 2

Implement robot behavior as state machines:

// FSM implementation
#include <rclcpp/rclcpp.hpp>
#include <memory>

enum class RobotState {
  IDLE,
  MOVING,
  GRASPING,
  PLACING,
  CHARGING
};

class RobotFSM : public rclcpp::Node {
 private:
  RobotState current_state_ = RobotState::IDLE;

 public:
  RobotFSM() : Node("robot_fsm") {
    timer_ = create_wall_timer(
      std::chrono::milliseconds(100),
      [this]() { update(); });
  }

 private:
  void update() {
    switch (current_state_) {
      case RobotState::IDLE:
        handle_idle();
        break;
      case RobotState::MOVING:
        handle_moving();
        break;
      case RobotState::GRASPING:
        handle_grasping();
        break;
      case RobotState::PLACING:
        handle_placing();
        break;
      case RobotState::CHARGING:
        handle_charging();
        break;
    }
  }

  void handle_idle() {
    // Check for new tasks
    if (has_new_task()) {
      current_state_ = RobotState::MOVING;
    }
  }

  void handle_moving() {
    // Move to target
    if (reached_target()) {
      current_state_ = RobotState::GRASPING;
    }
  }

  void handle_grasping() {
    // Grasp object
    if (object_grasped()) {
      current_state_ = RobotState::PLACING;
    }
  }

  void handle_placing() {
    // Place object
    if (object_placed()) {
      current_state_ = RobotState::IDLE;
    }
  }

  void handle_charging() {
    // Charge battery
    if (battery_full()) {
      current_state_ = RobotState::IDLE;
    }
  }

  rclcpp::TimerBase::SharedPtr timer_;
};

2. SMCL (Simple State Machine Control Language)

Use smcl_ros for declarative state machines:

# Install SMCL
ros2 package create my_state_machines

# robot_behavior.smcl
state_machine RobotBehavior {
  initial_state: idle

  state idle {
    on_entry: publish topic="/status" value="idle"
    transition to moving on task_available
  }

  state moving {
    on_entry: call_action "/move_to_target"
    transition to grasping on reach_target
    transition to idle on cancel
  }

  state grasping {
    on_entry: call_service "/grasp_object"
    transition to placing on grasp_success
    transition to moving on grasp_failure
  }

  state placing {
    on_entry: call_action "/place_object"
    transition to idle on place_success
  }
}

3. Behavior Trees (BT) with BehaviorTree.CPP

Hierarchical task planning with behavior trees:

// Behavior tree nodes
#include <behaviortree_cpp/bt_factory.h>

class MoveToTarget : public BT::AsyncBehaviorNode {
 public:
  MoveToTarget(const std::string& name) : BT::AsyncBehaviorNode(name, {}) {}

  BT::NodeStatus tick() override {
    RCLCPP_INFO(get_logger(), "Moving to target");
    // Send goal to navigation
    // Wait for result
    return BT::NodeStatus::SUCCESS;
  }

  void halt() override {}
};

class GraspObject : public BT::SyncActionNode {
 public:
  GraspObject(const std::string& name) : BT::SyncActionNode(name, {}) {}

  BT::NodeStatus tick() override {
    RCLCPP_INFO(get_logger(), "Grasping object");
    // Execute grasp
    return BT::NodeStatus::SUCCESS;
  }
};

class PlaceObject : public BT::AsyncBehaviorNode {
 public:
  PlaceObject(const std::string& name) : BT::AsyncBehaviorNode(name, {}) {}

  BT::NodeStatus tick() override {
    RCLCPP_INFO(get_logger(), "Placing object");
    return BT::NodeStatus::SUCCESS;
  }

  void halt() override {}
};

4. Behavior Tree XML Definition

Define complex behaviors in XML:

<?xml version="1.0" ?>
<root main_tree_to_execute="MainBehavior">
  <BehaviorTree ID="MainBehavior">
    <Sequence name="root">
      <!-- Wait for task -->
      <Action ID="WaitForTask"/>

      <!-- Main task sequence -->
      <Sequence name="pick_and_place">
        <!-- Move to object location -->
        <Action ID="MoveToTarget">
          <input name="target">object_location</input>
        </Action>

        <!-- Attempt grasp with retries -->
        <Retry num_attempts="3">
          <Action ID="GraspObject"/>
        </Retry>

        <!-- Move to placement location -->
        <Action ID="MoveToTarget">
          <input name="target">placement_location</input>
        </Action>

        <!-- Place object -->
        <Action ID="PlaceObject"/>
      </Sequence>

      <!-- Return to base -->
      <Action ID="ReturnToBase"/>
    </Sequence>
  </BehaviorTree>

  <!-- Fallback behavior for failures -->
  <BehaviorTree ID="ErrorRecovery">
    <Fallback name="recovery">
      <Action ID="RetryLastTask"/>
      <Action ID="ReturnToBase"/>
    </Fallback>
  </BehaviorTree>
</root>

5. Behavior Tree Execution Engine

Run behavior trees in ROS 2:

class BehaviorTreeExecutor : public rclcpp::Node {
 public:
  BehaviorTreeExecutor() : Node("bt_executor") {
    // Create factory
    BT::BehaviorTreeFactory factory;

    // Register custom nodes
    factory.registerNodeType<MoveToTarget>("MoveToTarget");
    factory.registerNodeType<GraspObject>("GraspObject");
    factory.registerNodeType<PlaceObject>("PlaceObject");

    // Load tree from file
    tree_ = factory.createTreeFromFile("robot_behavior.xml");

    // Create executor
    timer_ = create_wall_timer(
      std::chrono::milliseconds(50),
      [this]() { execute_tree(); });
  }

 private:
  void execute_tree() {
    BT::NodeStatus status = tree_.tickRoot();

    if (status == BT::NodeStatus::SUCCESS) {
      RCLCPP_INFO(get_logger(), "Task completed successfully");
    } else if (status == BT::NodeStatus::FAILURE) {
      RCLCPP_ERROR(get_logger(), "Task failed");
    }
  }

  BT::Tree tree_;
  rclcpp::TimerBase::SharedPtr timer_;
};

6. Hierarchical State Machines

Nested FSMs for complex behaviors:

// Hierarchical FSM
enum class HighLevelState {
  IDLE,
  WORKING,
  MAINTENANCE
};

enum class WorkingState {
  PICKING,
  TRANSPORTING,
  PLACING
};

class HierarchicalFSM {
 private:
  HighLevelState high_state_ = HighLevelState::IDLE;
  WorkingState work_state_;

  void update() {
    switch (high_state_) {
      case HighLevelState::IDLE:
        if (has_work()) high_state_ = HighLevelState::WORKING;
        break;

      case HighLevelState::WORKING:
        update_working_state();
        if (work_complete()) high_state_ = HighLevelState::IDLE;
        break;

      case HighLevelState::MAINTENANCE:
        perform_maintenance();
        break;
    }
  }

  void update_working_state() {
    switch (work_state_) {
      case WorkingState::PICKING:
        if (object_picked()) work_state_ = WorkingState::TRANSPORTING;
        break;
      case WorkingState::TRANSPORTING:
        if (destination_reached()) work_state_ = WorkingState::PLACING;
        break;
      case WorkingState::PLACING:
        if (object_placed()) work_state_ = WorkingState::PICKING;
        break;
    }
  }
};

7. Concurrent State Management

Handle parallel tasks:

// Parallel behavior execution
<?xml version="1.0" ?>
<BehaviorTree ID="ConcurrentTask">
  <Parallel success_threshold="2">
    <!-- Gripper control (parallel) -->
    <Action ID="MoveGripper">
      <input name="position">open</input>
    </Action>

    <!-- Joint movement (parallel) -->
    <Action ID="MoveJoint">
      <input name="joint">shoulder</input>
      <input name="angle">90</input>
    </Action>

    <!-- Sensor monitoring (parallel) -->
    <Action ID="MonitorForce"/>
  </Parallel>
</BehaviorTree>

8. Behavior Monitoring & Diagnostics

Monitor behavior execution:

# Monitor FSM/BT state
ros2 topic echo /behavior_status

# Behavior tree debugging
rqt_py_trees  # Visualize tree execution

# State machine introspection
ros2 service call /get_current_state std_srvs/Empty

# Log behavior transitions
ros2 bag record /behavior_status /behavior_diagnostics

9. Testing Behaviors

Unit test behavior logic:

// Test behavior tree
TEST(BehaviorTreeTest, pick_and_place_sequence) {
  BT::BehaviorTreeFactory factory;
  factory.registerNodeType<MockMoveToTarget>("MoveToTarget");
  factory.registerNodeType<MockGraspObject>("GraspObject");

  BT::Tree tree = factory.createTreeFromFile("robot_behavior.xml");

  // Simulate execution
  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS);
}

10. Best Practices

• FSM for simple flows: Use FSM for straightforward sequential behaviors
• BT for complex tasks: Behavior trees excel at hierarchical, modular tasks
• Modularity: Keep nodes focused on single responsibilities
• Monitoring: Log state transitions and provide diagnostics
• Testing: Unit test behavior nodes independently
• Timeouts: Implement timeouts to prevent infinite waits

Key Takeaways

Use finite state machines for simple, linear task flows. Implement behavior trees for complex, hierarchical task planning with fallback strategies. Combine both for robust autonomous robotics systems. Monitor behavior execution and log transitions for debugging and improvement.