ROS2 Task Execution Framework with BehaviorTree Integration

[andrewjong]

Software Design Document: Task Execution Framework for Mobile Robotics

Version: 1.1
Date: January 2026
Author: Andrew
Project: ROS2 Task Execution Framework with BehaviorTree Integration

---

Table of Contents

1. Executive Summary
2. System Overview
3. Architecture
4. Component Specifications
5. Interface Definitions
6. Data Flow
7. Implementation Details
8. Deployment
9. Testing Strategy
10. Future Extensions
11. Appendices

---

1. Executive Summary

1.1 Purpose

This document describes the software architecture for a modular, extensible task execution framework for mobile robots and embodied AI systems. The framework enables operators to dispatch high-level tasks (exploration, navigation, object search, etc.) through an intuitive GUI, with intelligent routing to specialized executor implementations, safety monitoring via behavior trees, and comprehensive health checking.

1.2 Goals

• Modularity: Tasks and executors are decoupled; new executors can be added without modifying core code
• Extensibility: Developers can easily add new task types and executor implementations via plugins
• Usability: Operators control the system through intuitive Rviz panels with real-time status feedback
• Robustness: Behavior tree integration provides retry logic, fallback strategies, and safety monitoring
• Efficiency: Lifecycle management ensures executors only consume resources when active
• Transparency: Operators see which executors are available and why others are unavailable
• Developer-Friendly: Executors are standalone ROS2 nodes that can be developed and tested independently

1.3 Key Features

• Plugin-based executor discovery using ROS2 pluginlib
• Executors as standalone ROS2 action server nodes for independent development
• ROS2 lifecycle nodes for resource-efficient executor management
• Simple dependency declaration and automatic health checking
• BehaviorTree.cpp integration for mission-level planning
• Custom Rviz panels for task dispatch and monitoring
• Multiple executor strategies per task type
• Real-time executor availability and health status

1.4 Design Philosophy

Standalone Executor Development: Each executor is a complete ROS2 node with its own action server. This allows developers to:

• Build and test executors independently of AirStack
• Use standard ROS2 patterns and tools
• Deploy executors on different machines if needed
• Debug executor logic independently from the framework

---

2. System Overview

2.1 System Context

┌────────────────────────────────────────────────────────────────┐
│                         Robot Platform                         │
│  ┌──────────────┐  ┌──────────────┐  ┌──────────────┐        │
│  │ Localization │  │   Mapping    │  │  Perception  │ ...    │
│  └──────────────┘  └──────────────┘  └──────────────┘        │
└───────────────────────────┬────────────────────────────────────┘
                            │ Topics (pose, map, obstacles, etc.)
                            ▼
┌────────────────────────────────────────────────────────────────┐
│               Task Execution Framework (This System)           │
│                                                                │
│  ┌─────────────┐  ┌──────────────┐  ┌────────────────────┐  │
│  │ Rviz Panels │→ │ BehaviorTree │→ │   Task Manager     │  │
│  │    (GUI)    │  │   Planner    │  │   + Executors      │  │
│  └─────────────┘  └──────────────┘  └────────────────────┘  │
└────────────────────────────────────────────────────────────────┘
                            │
                            ▼ Control commands
┌────────────────────────────────────────────────────────────────┐
│                    Robot Control Layer                         │
│           (Navigation stack, motion control, etc.)             │
└────────────────────────────────────────────────────────────────┘

2.2 Design Principles

1. Separation of Concerns

   • GUI Layer: User interaction and visualization
   • Behavior Layer: High-level mission logic and safety
   • Execution Layer: Task-specific algorithms and control
   • Robot Layer: Low-level sensing and actuation

2. Interface Standardization

   • All tasks use ROS2 actions with consistent Goal/Feedback/Result structure
   • All executors implement a common base class interface
   • All executors are standalone nodes with their own action servers
   • All panels follow common layout patterns

3. Simple Dependency Injection

   • Executors declare dependencies with minimal boilerplate
   • Dependencies grouped by type (topics, services, frames) and requirement level
   • System validates dependencies before execution
   • Missing dependencies are clearly communicated to users

4. Lazy Activation

   • All executors loaded at startup but remain in UNCONFIGURED state
   • Resources allocated only when executor is first used
   • Automatic deactivation after idle timeout

5. Graceful Degradation

   • Optional dependencies allow partial functionality
   • Behavior tree provides fallback strategies
   • System remains operational with subset of executors

6. Standalone Executor Development

   • Each executor is a complete, independent ROS2 node
   • Executors can be developed, tested, and debugged in isolation
   • Standard ROS2 action server pattern
   • Flexible deployment (same machine or distributed)

---

3. Architecture

3.1 Component Diagram

┌─────────────────────────────────────────────────────────────────────┐
│                           USER INTERFACE LAYER                      │
│  ┌──────────────┐  ┌──────────────┐  ┌──────────────┐             │
│  │   Explore    │  │   Navigate   │  │    Patrol    │  ...        │
│  │  Rviz Panel  │  │  Rviz Panel  │  │  Rviz Panel  │             │
│  └──────┬───────┘  └──────┬───────┘  └──────┬───────┘             │
│         │ TaskRequest     │                  │                      │
└─────────┼─────────────────┼──────────────────┼──────────────────────┘
          │                 │                  │
          │ /bt_mission_planner/task_requests  │
          ▼                 ▼                  ▼
┌─────────────────────────────────────────────────────────────────────┐
│                        BEHAVIOR LAYER                               │
│  ┌───────────────────────────────────────────────────────────────┐ │
│  │            BehaviorTree Mission Planner                       │ │
│  │  ┌─────────────────┐  ┌─────────────────┐  ┌──────────────┐ │ │
│  │  │ WaitForTask     │→ │ Validate        │→ │ Execute Task │ │ │
│  │  │ Request Node    │  │ (Battery, GPS)  │  │ w/ Retry     │ │ │
│  │  └─────────────────┘  └─────────────────┘  └──────┬───────┘ │ │
│  └─────────────────────────────────────────────────────┼─────────┘ │
│                                                        │ Action     │
└────────────────────────────────────────────────────────┼───────────┘
                                                         │
          /task_manager/{task_type}_task                 │
                                                         ▼
┌─────────────────────────────────────────────────────────────────────┐
│                      ORCHESTRATION LAYER                            │
│  ┌───────────────────────────────────────────────────────────────┐ │
│  │                    Task Manager Node                          │ │
│  │                                                               │ │
│  │  ┌──────────────┐  ┌──────────────┐  ┌──────────────┐      │ │
│  │  │  Pluginlib   │  │  Lifecycle   │  │    Health    │      │ │
│  │  │  Loader      │  │  Manager     │  │   Monitor    │      │ │
│  │  └──────────────┘  └──────────────┘  └──────────────┘      │ │
│  │                                                               │ │
│  │  Action Servers: /explore_task, /navigate_task, ...          │ │
│  │  (Routes to executor action servers)                         │ │
│  └───────────────────────────────┬───────────────────────────────┘ │
└──────────────────────────────────┼─────────────────────────────────┘
                                   │ Forwards to executor's action
                                   ▼
┌─────────────────────────────────────────────────────────────────────┐
│                       EXECUTION LAYER                               │
│                  (Standalone ROS2 Nodes)                            │
│                                                                     │
│  ┌─────────────────────────────────────────────────────────────┐  │
│  │  TaskExplore Executors (BaseExplorer)                       │  │
│  │  ┌──────────────────────────────────────────────────────┐  │  │
│  │  │ FrontierExplorer Node                                │  │  │
│  │  │ - Action Server: ~/execute_explore                   │  │  │
│  │  │ - Subscribes: /map, /robot_pose                      │  │  │
│  │  │ - Plugin: explorers::FrontierExplorer                │  │  │
│  │  └──────────────────────────────────────────────────────┘  │  │
│  │  ┌──────────────────────────────────────────────────────┐  │  │
│  │  │ RandomWalkExplorer Node                              │  │  │
│  │  │ - Action Server: ~/execute_explore                   │  │  │
│  │  │ - Plugin: explorers::RandomWalkExplorer              │  │  │
│  │  └──────────────────────────────────────────────────────┘  │  │
│  └─────────────────────────────────────────────────────────────┘  │
│                                                                     │
│  ┌─────────────────────────────────────────────────────────────┐  │
│  │  TaskNavigate Executors (BaseNavigator)                     │  │
│  │  ┌──────────────────────────────────────────────────────┐  │  │
│  │  │ AStarNavigator Node                                  │  │  │
│  │  │ - Action Server: ~/execute_navigate                  │  │  │
│  │  └──────────────────────────────────────────────────────┘  │  │
│  │  ┌──────────────────────────────────────────────────────┐  │  │
│  │  │ RRTNavigator Node                                    │  │  │
│  │  │ - Action Server: ~/execute_navigate                  │  │  │
│  │  └──────────────────────────────────────────────────────┘  │  │
│  └─────────────────────────────────────────────────────────────┘  │
│                                                                     │
│  [Additional task types...]                                        │
└─────────────────────────────────────────────────────────────────────┘

3.2 Layer Responsibilities

3.2.1 User Interface Layer

• Present task configuration forms
• Display executor availability with tooltips
• Show real-time task progress
• Publish TaskRequest messages on user action
• Subscribe to TaskResponse for status updates

3.2.2 Behavior Layer

• Monitor for incoming TaskRequest messages
• Validate preconditions (battery, GPS, executor health)
• Execute tasks with retry and fallback logic
• Handle task preemption and cancellation
• Provide mission-level sequencing and coordination

3.2.3 Orchestration Layer

• Discover executor plugins via pluginlib
• Manage executor lifecycle states (unconfigured → configured → active)
• Monitor executor health and dependencies
• Route action requests to appropriate executor action servers
• Publish executor status for GUI consumption
• Handle executor activation/deactivation

3.2.4 Execution Layer

• Implement task-specific algorithms as standalone ROS2 nodes
• Provide action servers for task execution
• Declare required/optional dependencies with simple syntax
• Report health status
• Execute tasks independently of framework
• Provide feedback during execution

3.3 Technology Stack

Component	Technology	Version
Middleware	ROS2	Humble/Iron
Build System	colcon + ament_cmake	-
Plugin System	pluginlib	ROS2
Node Lifecycle	rclcpp_lifecycle	ROS2
Actions	rclcpp_action	ROS2
Behavior Trees	BehaviorTree.CPP	4.x
GUI Framework	Qt5 + Rviz2	-
Serialization	JSON (nlohmann)	3.x
Language	C++17	-

---

4. Component Specifications

4.1 Task Manager Node

Package: task_framework_core
Executable: task_manager_node
Node Name: task_manager

4.1.1 Responsibilities

1. Executor Discovery: Load all executor plugins using pluginlib
2. Lifecycle Management: Configure/activate/deactivate executors on demand
3. Health Monitoring: Periodically check executor dependencies and health
4. Action Routing: Provide action servers for each task type and forward to executor action servers
5. Status Publishing: Publish executor availability for GUI consumption

4.1.2 Key Classes

class TaskManager : public rclcpp::Node {
public:
  TaskManager();
  
private:
  // Executor management
  void discoverExecutors();
  void initializeExecutors();
  std::shared_ptr<BaseTaskExecutor> activateExecutor(const std::string& name);
  void deactivateExecutor(const std::string& name);
  
  // Health monitoring
  void checkExecutorHealth();
  ExecutorStatus checkSingleExecutor(const std::string& name);
  
  // Action server creation
  template<typename ActionType>
  void createTaskActionServer(const std::string& action_name,
                               const std::string& task_type);
  
  template<typename ActionType>
  void executeTask(std::shared_ptr<GoalHandle> goal_handle,
                   const std::string& task_type);
  
  // Executor selection
  std::shared_ptr<BaseTaskExecutor> selectExecutor(
    const std::string& task_type,
    const std::string& requested_name = "");
  
  // Data structures
  struct ExecutorInfo {
    std::shared_ptr<BaseTaskExecutor> node;
    ExecutorState state;  // SLEEPING, READY, ACTIVE
    std::chrono::time_point<std::chrono::steady_clock> last_used;
    ExecutorCapabilities capabilities;
  };
  
  pluginlib::ClassLoader<BaseTaskExecutor> executor_loader_;
  std::map<std::string, ExecutorInfo> executors_;
  std::map<std::string, rclcpp_action::ServerBase::SharedPtr> action_servers_;
  
  rclcpp::Publisher<ExecutorStatusArray>::SharedPtr status_pub_;
  rclcpp::TimerBase::SharedPtr health_check_timer_;
  rclcpp::TimerBase::SharedPtr cleanup_timer_;
};

4.1.3 Parameters

task_manager:
  executor_mode: "lazy_activation"  # all_active, lazy_activation, on_demand
  health_check_rate: 0.5  # Hz
  idle_timeout_minutes: 5
  auto_cleanup_enabled: true

4.1.4 Published Topics

Topic	Type	Description
~/executor_status	ExecutorStatusArray	Periodic executor health/availability

4.1.5 Action Servers

Action	Type	Description
~/explore_task	task_framework_msgs/action/Explore	Exploration tasks
~/navigate_task	task_framework_msgs/action/Navigate	Navigation tasks
~/patrol_task	task_framework_msgs/action/Patrol	Patrol tasks
~/object_search_task	task_framework_msgs/action/ObjectSearch	Object search tasks
~/3d_coverage_task	task_framework_msgs/action/Coverage3D	3D coverage tasks

4.1.6 Services

Service	Type	Description
~/list_executors	ListExecutors	Query available executors for task type
~/check_executor_health	CheckExecutorHealth	Check specific executor health
~/force_executor_reload	Trigger	Reload executor plugins

---

4.2 Base Task Executor

Package: task_framework_core
Header: task_framework/base_task_executor.hpp

4.2.1 Purpose

Abstract base class for all task executors, combining:

• ROS2 lifecycle node functionality
• Simple dependency declaration system
• Health checking capabilities
• Plugin interface for dynamic loading
• Action server for standalone operation

4.2.2 Class Definition

namespace task_framework {

class BaseTaskExecutor : public rclcpp_lifecycle::LifecycleNode {
public:
  using CallbackReturn = rclcpp_lifecycle::node_interfaces::
                         LifecycleNodeInterface::CallbackReturn;
  
  explicit BaseTaskExecutor(const std::string& node_name);
  virtual ~BaseTaskExecutor() = default;
  
  // ===== MUST IMPLEMENT =====
  
  // Declare what this executor can do and needs
  virtual ExecutorCapabilities getCapabilities() const = 0;
  
  // ===== LIFECYCLE HOOKS (OPTIONAL OVERRIDE) =====
  
  virtual bool onConfigure() { return true; }
  virtual bool onActivate() { return true; }
  virtual void onDeactivate() {}
  virtual void onCleanup() {}
  
  // ===== HEALTH CHECKING =====
  
  // Check current health status
  virtual ExecutorStatus checkHealth();
  
protected:
  // Convenience methods for dependency checking
  bool isTopicAvailable(const std::string& topic);
  bool isServiceAvailable(const std::string& service);
  bool isTfFrameAvailable(const std::string& frame_id);
  
private:
  // Lifecycle callbacks (final)
  CallbackReturn on_configure(const State&) final override;
  CallbackReturn on_activate(const State&) final override;
  CallbackReturn on_deactivate(const State&) final override;
  CallbackReturn on_cleanup(const State&) final override;
};

} // namespace task_framework

4.2.3 Executor Lifecycle States

UNCONFIGURED (Sleeping)
  │ Resources: Minimal (node exists, no subscriptions)
  │ Call: configure()
  ▼
INACTIVE (Ready)
  │ Resources: Low (subscriptions created but not active)
  │ Call: activate()
  ▼
ACTIVE (Running)
  │ Resources: Full (processing callbacks, executing tasks)
  │ Call: deactivate() [after idle timeout]
  ▼
INACTIVE (Ready)
  │ Call: cleanup() [optional]
  ▼
UNCONFIGURED (Sleeping)

4.2.4 Health Checking Implementation

ExecutorStatus BaseTaskExecutor::checkHealth() {
  ExecutorStatus status;
  status.executor_name = get_name();
  status.last_checked = now();
  
  // Check lifecycle state
  if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
    status.health = ExecutorHealth::UNCONFIGURED;
    status.detailed_message = "Executor not activated";
    return status;
  }
  
  auto caps = getCapabilities();
  
  // Check required topics
  for (const auto& dep : caps.required_topics) {
    if (!isTopicAvailable(dep.name)) {
      status.health = ExecutorHealth::UNAVAILABLE;
      status.missing_dependencies.push_back(
        dep.name + " - " + dep.description
      );
    }
  }
  
  // Check optional topics (warnings only)
  for (const auto& dep : caps.optional_topics) {
    if (!isTopicAvailable(dep.name)) {
      if (status.health == ExecutorHealth::UNCONFIGURED) {
        status.health = ExecutorHealth::DEGRADED;
      }
      status.warnings.push_back(
        "Optional: " + dep.name + " - " + dep.description
      );
    }
  }
  
  // Check required services
  for (const auto& dep : caps.required_services) {
    if (!isServiceAvailable(dep.name)) {
      status.health = ExecutorHealth::UNAVAILABLE;
      status.missing_dependencies.push_back(
        "Service: " + dep.name + " - " + dep.description
      );
    }
  }
  
  // Check required frames
  for (const auto& dep : caps.required_frames) {
    if (!isTfFrameAvailable(dep.name)) {
      status.health = ExecutorHealth::UNAVAILABLE;
      status.missing_dependencies.push_back(
        "TF frame: " + dep.name + " - " + dep.description
      );
    }
  }
  
  // If no issues found
  if (status.missing_dependencies.empty()) {
    status.health = (status.warnings.empty()) ? 
      ExecutorHealth::HEALTHY : ExecutorHealth::DEGRADED;
    status.detailed_message = "All systems operational";
  } else {
    status.detailed_message = "Missing required dependencies";
  }
  
  return status;
}

---

4.3 Executor Implementation Example

Package: exploration_executors
Plugin: FrontierExplorer

#include "task_framework/base_task_executor.hpp"
#include <pluginlib/class_list_macros.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include "task_framework_msgs/action/explore.hpp"

namespace explorers {

class FrontierExplorer : public task_framework::BaseTaskExecutor {
public:
  using ExploreAction = task_framework_msgs::action::Explore;
  using GoalHandle = rclcpp_action::ServerGoalHandle<ExploreAction>;
  
  FrontierExplorer() : BaseTaskExecutor("frontier_explorer") {}
  
  // ===== Capability Declaration =====
  
  task_framework::ExecutorCapabilities getCapabilities() const override {
    task_framework::ExecutorCapabilities caps;
    caps.executor_name = "FrontierExplorer";
    caps.compatible_task_type = "TaskExplore";
    caps.description = "Frontier-based exploration using occupancy grid";
    
    // Required topics - just name and description!
    caps.required_topics = {
      {"/map", "Occupancy grid map for frontier detection"},
      {"/robot_pose", "Current robot pose for planning"}
    };
    
    // Required TF frames
    caps.required_frames = {
      {"map", "Global map coordinate frame"}
    };
    
    // Optional topics (enable advanced features)
    caps.optional_topics = {
      {"/semantic_map", "Enables semantic-aware exploration"}
    };
    
    return caps;
  }
  
protected:
  // ===== Lifecycle Callbacks =====
  
  bool onConfigure() override {
    RCLCPP_INFO(get_logger(), "Configuring FrontierExplorer");
    
    // Create subscriptions
    map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
      "/map", 10, 
      std::bind(&FrontierExplorer::mapCallback, this, std::placeholders::_1));
    
    pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
      "/robot_pose", 10,
      std::bind(&FrontierExplorer::poseCallback, this, std::placeholders::_1));
    
    // Create action server (this executor is a standalone node!)
    explore_server_ = rclcpp_action::create_server<ExploreAction>(
      get_node_base_interface(),
      get_node_clock_interface(),
      get_node_logging_interface(),
      get_node_waitables_interface(),
      "~/execute_explore",
      std::bind(&FrontierExplorer::handleGoal, this, 
                std::placeholders::_1, std::placeholders::_2),
      std::bind(&FrontierExplorer::handleCancel, this, 
                std::placeholders::_1),
      std::bind(&FrontierExplorer::handleAccepted, this, 
                std::placeholders::_1));
    
    RCLCPP_INFO(get_logger(), "FrontierExplorer configured");
    RCLCPP_INFO(get_logger(), "Action server available at: ~/execute_explore");
    return true;
  }
  
  bool onActivate() override {
    RCLCPP_INFO(get_logger(), "FrontierExplorer activated");
    return true;
  }
  
  void onDeactivate() override {
    RCLCPP_INFO(get_logger(), "FrontierExplorer deactivated");
  }
  
  void onCleanup() override {
    RCLCPP_INFO(get_logger(), "FrontierExplorer cleaned up");
  }
  
private:
  // ===== Subscriptions =====
  
  void mapCallback(nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
    current_map_ = msg;
  }
  
  void poseCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg) {
    current_pose_ = msg;
  }
  
  // ===== Action Server Handlers =====
  
  rclcpp_action::GoalResponse handleGoal(
      const rclcpp_action::GoalUUID& uuid,
      std::shared_ptr<const ExploreAction::Goal> goal) {
    
    RCLCPP_INFO(get_logger(), "Received exploration goal request");
    
    // Validate goal
    if (goal->timeout <= 0) {
      RCLCPP_WARN(get_logger(), "Invalid timeout, rejecting goal");
      return rclcpp_action::GoalResponse::REJECT;
    }
    
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }
  
  rclcpp_action::CancelResponse handleCancel(
      const std::shared_ptr<GoalHandle> goal_handle) {
    RCLCPP_INFO(get_logger(), "Received cancel request");
    return rclcpp_action::CancelResponse::ACCEPT;
  }
  
  void handleAccepted(const std::shared_ptr<GoalHandle> goal_handle) {
    // Execute in separate thread to not block
    std::thread{
      std::bind(&FrontierExplorer::executeExploration, this, 
                std::placeholders::_1), 
      goal_handle
    }.detach();
  }
  
  void executeExploration(const std::shared_ptr<GoalHandle> goal_handle) {
    RCLCPP_INFO(get_logger(), "Executing exploration task");
    
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<ExploreAction::Feedback>();
    auto result = std::make_shared<ExploreAction::Result>();
    
    rclcpp::Rate rate(10);  // 10 Hz
    
    // Main exploration loop
    while (rclcpp::ok()) {
      // Check for cancellation
      if (goal_handle->is_canceling()) {
        result->success = false;
        result->error_message = "Goal cancelled by client";
        goal_handle->canceled(result);
        RCLCPP_INFO(get_logger(), "Exploration cancelled");
        return;
      }
      
      // Execute frontier detection and navigation
      // ... your exploration algorithm here ...
      
      // Publish feedback
      feedback->current_coverage = 0.45;  // Example
      feedback->frontiers_found = 12;
      feedback->frontiers_remaining = 5;
      goal_handle->publish_feedback(feedback);
      
      // Check completion
      if (feedback->current_coverage >= goal->coverage_threshold) {
        result->success = true;
        result->final_coverage = feedback->current_coverage;
        result->frontiers_explored = feedback->frontiers_found;
        goal_handle->succeed(result);
        RCLCPP_INFO(get_logger(), "Exploration completed successfully");
        return;
      }
      
      rate.sleep();
    }
  }
  
  // ===== Member Variables =====
  
  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
  
  rclcpp_action::Server<ExploreAction>::SharedPtr explore_server_;
  
  nav_msgs::msg::OccupancyGrid::SharedPtr current_map_;
  geometry_msgs::msg::PoseStamped::SharedPtr current_pose_;
};

} // namespace explorers

// Register as plugin
PLUGINLIB_EXPORT_CLASS(explorers::FrontierExplorer, 
                       task_framework::BaseTaskExecutor)

Plugin Description XML (explorers_plugins.xml):

<library path="frontier_explorer">
  <class type="explorers::FrontierExplorer" 
         base_class_type="task_framework::BaseTaskExecutor">
    <description>
      Frontier-based exploration algorithm for unknown environment mapping.
      Standalone ROS2 node with action server at ~/execute_explore.
    </description>
  </class>
</library>

Standalone Usage (for development/testing):

# Developer can run and test executor independently
ros2 run exploration_executors frontier_explorer_node

# Test with action client
ros2 action send_goal /frontier_explorer/execute_explore \
  task_framework_msgs/action/Explore \
  "{boundary: 'POLYGON((0 0, 10 0, 10 10, 0 10))', timeout: 300.0, coverage_threshold: 0.9}"

---

4.4 Behavior Tree Mission Planner

Package: task_framework_bt
Executable: bt_mission_planner
Node Name: bt_mission_planner

4.4.1 Responsibilities

1. Load and execute behavior tree definitions (XML)
2. Listen for task requests from Rviz panels
3. Validate task preconditions (battery, GPS, executor health)
4. Execute tasks with retry, timeout, and fallback logic
5. Send task status updates back to panels
6. Handle task cancellation/preemption
7. Coordinate multi-task missions

4.4.2 Custom BT Nodes

Action Nodes (execute tasks):

• ExploreActionNode - Calls exploration action
• NavigateActionNode - Calls navigation action
• PatrolActionNode - Calls patrol action
• ObjectSearchActionNode - Calls object search action
• Coverage3DActionNode - Calls 3D coverage action

Condition Nodes (check state):

• WaitForTaskRequest - Monitors for incoming TaskRequest messages
• ExecutorAvailableCondition - Checks if executor is healthy
• BatteryLevelCondition - Checks battery charge
• GPSQualityCondition - Checks GPS satellite count/accuracy
• TemperatureCondition - Checks system temperature

Utility Nodes:

• SelectExecutorAction - Chooses best executor based on criteria
• LogTaskResult - Logs task completion to file
• SendTaskResponse - Publishes TaskResponse message

4.4.3 BT Node: WaitForTaskRequest

class WaitForTaskRequest : public BT::ConditionNode {
public:
  static BT::PortsList providedPorts() {
    return {
      BT::InputPort<std::string>("task_type", 
        "Filter by task type (empty = any)"),
      BT::OutputPort<std::string>("request_id"),
      BT::OutputPort<std::string>("task_type_out"),
      BT::OutputPort<std::string>("parameters_json"),
      BT::OutputPort<std::string>("requested_executor"),
      BT::OutputPort<uint8_t>("priority")
    };
  }
  
  BT::NodeStatus tick() override {
    rclcpp::spin_some(node_);
    
    std::lock_guard<std::mutex> lock(queue_mutex_);
    
    if (request_queue_.empty()) {
      return BT::NodeStatus::FAILURE;
    }
    
    auto request = request_queue_.front();
    request_queue_.pop();
    
    // Output to blackboard
    setOutput("request_id", request.request_id);
    setOutput("task_type_out", request.task_type);
    setOutput("parameters_json", request.parameters_json);
    setOutput("requested_executor", request.requested_executor);
    setOutput("priority", request.priority);
    
    // Send acceptance response
    sendResponse(request.request_id, 
                 TaskResponse::STATUS_ACCEPTED,
                 "Task accepted by behavior tree");
    
    return BT::NodeStatus::SUCCESS;
  }
  
private:
  void requestCallback(TaskRequest::SharedPtr msg) {
    std::lock_guard<std::mutex> lock(queue_mutex_);
    request_queue_.push(*msg);
  }
  
  rclcpp::Node::SharedPtr node_;
  rclcpp::Subscription<TaskRequest>::SharedPtr request_sub_;
  rclcpp::Publisher<TaskResponse>::SharedPtr response_pub_;
  std::mutex queue_mutex_;
  std::queue<TaskRequest> request_queue_;
};

4.4.4 Example Behavior Tree

File: reactive_exploration_mission.xml

<root BTCPP_format="4">
  <BehaviorTree ID="ReactiveExplorationMission">
    
    <ReactiveSequence>
      
      <!-- Emergency monitoring (runs in parallel) -->
      <Fallback name="SafetyCheck">
        <Sequence>
          <Condition ID="BatteryLevel" min_level="0.20"/>
          <Condition ID="GPSQuality" min_satellites="6"/>
          <Condition ID="TemperatureOk" max_temp="65"/>
        </Sequence>
        
        <!-- Emergency return home -->
        <Sequence>
          <Action ID="LogMessage" 
                  message="EMERGENCY: Safety limits exceeded, RTH"/>
          <Action ID="Navigate"
                  server_name="/task_manager/navigate_task"
                  goal_pose="{home_pose}"
                  timeout="300"/>
        </Sequence>
      </Fallback>
      
      <!-- Main task execution loop -->
      <Fallback name="TaskDispatcher">
        
        <!-- Handle exploration requests -->
        <Sequence name="HandleExploration">
          <Condition ID="WaitForTaskRequest" 
                     task_type="TaskExplore"
                     request_id="{request_id}"
                     parameters_json="{params}"
                     requested_executor="{executor}"/>
          
          <Fallback name="ExploreWithFallback">
            <!-- Try requested executor -->
            <Sequence>
              <Condition ID="ExecutorAvailable" 
                         executor_name="{executor}"/>
              <RetryUntilSuccessful num_attempts="3">
                <Action ID="Explore"
                        server_name="/task_manager/explore_task"
                        request_id="{request_id}"
                        parameters_json="{params}"
                        requested_executor="{executor}"/>
              </RetryUntilSuccessful>
            </Sequence>
            
            <!-- Auto-select alternative -->
            <Sequence>
              <Action ID="SelectExecutor" 
                      task_type="TaskExplore"
                      optimization="best_performance"
                      selected_executor="{executor}"/>
              <RetryUntilSuccessful num_attempts="2">
                <Action ID="Explore"
                        server_name="/task_manager/explore_task"
                        request_id="{request_id}"
                        parameters_json="{params}"
                        requested_executor="{executor}"/>
              </RetryUntilSuccessful>
            </Sequence>
          </Fallback>
        </Sequence>
        
        <!-- Handle navigation requests -->
        <Sequence name="HandleNavigation">
          <Condition ID="WaitForTaskRequest" 
                     task_type="TaskNavigate"
                     request_id="{nav_id}"
                     parameters_json="{nav_params}"/>
          
          <Action ID="Navigate"
                  server_name="/task_manager/navigate_task"
                  request_id="{nav_id}"
                  parameters_json="{nav_params}"/>
        </Sequence>
        
        <!-- Idle if no requests -->
        <AlwaysSuccess/>
      </Fallback>
      
    </ReactiveSequence>
    
  </BehaviorTree>
</root>

4.4.5 Topics

Topic	Type	Direction	Description
/bt_mission_planner/task_requests	TaskRequest	Subscribe	Incoming task requests from panels
/bt_mission_planner/task_responses	TaskResponse	Publish	Status updates to panels

4.4.6 Parameters

bt_mission_planner:
  tree_file: "reactive_exploration_mission.xml"
  tick_rate: 10.0  # Hz
  enable_groot: true
  groot_publisher_port: 1666
  groot_server_port: 1667

---

4.5 Rviz Panel

Package: task_framework_rviz
Plugin: ExplorePanel

4.5.1 UI Layout

┌────────────────────────────────────────┐
│      Exploration Task Panel            │
├────────────────────────────────────────┤
│  Exploration Parameters                │
│  ┌──────────────────────────────────┐ │
│  │ Boundary: [________text______]   │ │
│  │ Timeout (s): [300▲▼]             │ │
│  │ Coverage: [0.90▲▼]               │ │
│  └──────────────────────────────────┘ │
├────────────────────────────────────────┤
│  Executor Selection                    │
│  ┌──────────────────────────────────┐ │
│  │ ✓ FrontierExplorer          ▼   │ │
│  │ ✗ RandomWalkExplorer (unavail)  │ │
│  └──────────────────────────────────┘ │
│                                        │
│  Status: All dependencies met          │
├────────────────────────────────────────┤
│  [Dispatch Task]  [Cancel]             │
├────────────────────────────────────────┤
│  Task Status                           │
│  ┌──────────────────────────────────┐ │
│  │ Executing: 45% coverage          │ │
│  │ [████████░░░░░░░░░░] 45%         │ │
│  └──────────────────────────────────┘ │
└────────────────────────────────────────┘

4.5.2 Key Features

1. Parameter Input

   • Task-specific parameter forms
   • Input validation
   • Tooltips with parameter descriptions

2. Executor Selection

   • Dropdown with available executors
   • Visual indicators (✓/✗) for health
   • Disabled entries for unavailable executors
   • Tooltips showing missing dependencies

3. Task Dispatch

   • Dispatch button (enabled only if executor available)
   • Cancel button (enabled during execution)
   • Publishes TaskRequest on dispatch

4. Status Display

   • Real-time status messages
   • Progress bar
   • Color-coded status (green/yellow/red)
   • Task result display

4.5.3 Implementation Snippet

class ExplorePanel : public rviz_common::Panel {
  Q_OBJECT
  
private Q_SLOTS:
  void onDispatchClicked() {
    // Create task request
    task_framework_msgs::msg::TaskRequest request;
    request.request_id = generateRequestId();
    request.task_type = "TaskExplore";
    request.priority = TaskRequest::PRIORITY_NORMAL;
    request.stamp = node_->now();
    
    // Get selected executor
    int idx = executor_combo_->currentIndex();
    if (idx >= 0 && idx < executor_statuses_.size()) {
      request.requested_executor = executor_statuses_[idx].executor_name;
    }
    
    // Pack parameters into JSON
    nlohmann::json params;
    params["boundary"] = boundary_input_->text().toStdString();
    params["timeout"] = timeout_spin_->value();
    params["coverage_threshold"] = coverage_spin_->value();
    request.parameters_json = params.dump();
    
    // Publish request
    task_request_pub_->publish(request);
    
    // Update UI
    dispatch_button_->setEnabled(false);
    cancel_button_->setEnabled(true);
    task_status_label_->setText("Task dispatched, waiting for BT...");
  }
  
  void executorStatusCallback(const ExecutorStatusArray::SharedPtr msg) {
    executor_statuses_.clear();
    executor_combo_->clear();
    
    bool has_available = false;
    
    for (const auto& status : msg->statuses) {
      if (status.compatible_task_type == "TaskExplore") {
        executor_statuses_.push_back(status);
        
        QString display_name = QString::fromStdString(status.executor_name);
        bool is_available = (status.health == ExecutorStatus::HEALTHY);
        
        if (is_available) {
          display_name = "✓ " + display_name;
          has_available = true;
        } else {
          display_name = "✗ " + display_name + " (unavailable)";
        }
        
        executor_combo_->addItem(display_name);
        
        // Disable unavailable executors and add tooltip
        if (!is_available) {
          auto model = qobject_cast<QStandardItemModel*>(executor_combo_->model());
          auto item = model->item(executor_combo_->count() - 1);
          item->setEnabled(false);
          
          QString tooltip = QString::fromStdString(status.detailed_message);
          if (!status.missing_dependencies.empty()) {
            tooltip += "\n\nMissing:\n";
            for (const auto& dep : status.missing_dependencies) {
              tooltip += "• " + QString::fromStdString(dep) + "\n";
            }
          }
          item->setToolTip(tooltip);
        }
      }
    }
    
    dispatch_button_->setEnabled(has_available && !cancel_button_->isEnabled());
  }
};

4.5.4 Topics

Topic	Type	Direction	Description
/bt_mission_planner/task_requests	TaskRequest	Publish	Send task dispatch requests
/bt_mission_planner/task_responses	TaskResponse	Subscribe	Receive status updates
/task_manager/executor_status	ExecutorStatusArray	Subscribe	Monitor executor health

---

5. Interface Definitions

5.1 ROS2 Action Definitions

5.1.1 Explore Action

File: task_framework_msgs/action/Explore.action

# Goal
string boundary              # Polygon definition (WKT or JSON)
float32 timeout              # Maximum execution time (seconds)
float32 coverage_threshold   # Completion threshold (0.0-1.0)

---
# Result
bool success
float32 final_coverage       # Achieved coverage (0.0-1.0)
int32 frontiers_explored     # Number of frontiers visited
float32 distance_traveled    # Total distance (meters)
string error_message         # Error details if failed

---
# Feedback
float32 current_coverage     # Current coverage (0.0-1.0)
int32 frontiers_found        # Frontiers discovered
int32 frontiers_remaining    # Frontiers yet to visit
geometry_msgs/PoseStamped current_pose

5.1.2 Navigate Action

File: task_framework_msgs/action/Navigate.action

# Goal
geometry_msgs/PoseStamped goal_pose
float32 timeout
float32 position_tolerance    # meters
float32 orientation_tolerance # radians

---
# Result
bool success
float32 final_distance_error  # meters
float32 final_angle_error     # radians
float32 time_taken            # seconds
string error_message

---
# Feedback
geometry_msgs/PoseStamped current_pose
float32 distance_to_goal      # meters
float32 estimated_time        # seconds
string current_phase          # "planning", "executing", "recovering"

5.1.3 ObjectSearch Action

File: task_framework_msgs/action/ObjectSearch.action

# Goal
string target_class           # Object class to search for
string search_area            # Polygon or "current_map"
float32 timeout
int32 max_detections          # Stop after N detections (-1 = unlimited)

---
# Result
bool success
int32 objects_found
geometry_msgs/PoseStamped[] object_poses
float32[] detection_confidences
string error_message

---
# Feedback
int32 objects_detected
float32 area_searched         # Percentage (0.0-1.0)
geometry_msgs/PoseStamped current_pose

5.2 Message Definitions

5.2.1 Dependency

File: task_framework_msgs/msg/Dependency.msg

# Dependency declaration
string name          # Topic/service/frame name
string description   # Human-readable description for GUI tooltips

5.2.2 ExecutorCapabilities

File: task_framework_msgs/msg/ExecutorCapabilities.msg

# Executor capability declaration with dependencies

string executor_name
string compatible_task_type
string description

# Dependency lists
Dependency[] required_topics
Dependency[] optional_topics
Dependency[] required_services
Dependency[] optional_services
Dependency[] required_frames
Dependency[] optional_frames

5.2.3 ExecutorStatus

File: task_framework_msgs/msg/ExecutorStatus.msg

string executor_name
string compatible_task_type
string description

uint8 HEALTHY = 0
uint8 DEGRADED = 1
uint8 UNAVAILABLE = 2
uint8 ERROR = 3
uint8 UNCONFIGURED = 4

uint8 health
string[] missing_dependencies  # Formatted as "name - description"
string[] warnings
string detailed_message
builtin_interfaces/Time last_checked

5.2.4 ExecutorStatusArray

File: task_framework_msgs/msg/ExecutorStatusArray.msg

std_msgs/Header header
ExecutorStatus[] statuses

5.2.5 TaskRequest

File: task_framework_msgs/msg/TaskRequest.msg

string request_id
string task_type              # "TaskExplore", "TaskNavigate", etc.
string parameters_json        # JSON-encoded parameters
string requested_executor     # Specific executor (empty = auto)

uint8 PRIORITY_LOW = 0
uint8 PRIORITY_NORMAL = 1
uint8 PRIORITY_HIGH = 2
uint8 PRIORITY_EMERGENCY = 3
uint8 priority

builtin_interfaces/Time stamp

5.2.6 TaskResponse

File: task_framework_msgs/msg/TaskResponse.msg

string request_id
string task_type

uint8 STATUS_ACCEPTED = 0
uint8 STATUS_REJECTED = 1
uint8 STATUS_EXECUTING = 2
uint8 STATUS_SUCCEEDED = 3
uint8 STATUS_FAILED = 4
uint8 STATUS_PREEMPTED = 5
uint8 status

string message
float32 progress              # 0.0 to 1.0

5.3 Service Definitions

5.3.1 ListExecutors

File: task_framework_msgs/srv/ListExecutors.srv

# Request
string task_type    # Filter by task type (empty = all)

---
# Response
ExecutorInfo[] executors

# ExecutorInfo structure (inline)
string name
string capabilities
bool is_available
string unavailable_reason

5.3.2 CheckExecutorHealth

File: task_framework_msgs/srv/CheckExecutorHealth.srv

# Request
string executor_name   # Empty for all executors
string task_type       # Empty for all task types

---
# Response
bool is_available
ExecutorStatus status

---

6. Data Flow

6.1 Task Dispatch Flow

1. User configures task in Rviz Panel
   │
   ├─> Fills in parameters (boundary, timeout, etc.)
   ├─> Selects executor from dropdown
   └─> Clicks "Dispatch"
   │
   ▼
2. Rviz Panel publishes TaskRequest
   │
   │  Topic: /bt_mission_planner/task_requests
   │  Message: {request_id, task_type, parameters_json, 
   │             requested_executor, priority}
   │
   ▼
3. BehaviorTree WaitForTaskRequest node receives request
   │
   ├─> Stores in internal queue
   ├─> Outputs to blackboard
   └─> Returns SUCCESS
   │
   ▼
4. BehaviorTree validates preconditions
   │
   ├─> BatteryLevel condition checks battery
   ├─> GPSQuality condition checks GPS
   └─> ExecutorAvailable checks executor health
   │
   ▼
5. BehaviorTree publishes acceptance
   │
   │  Topic: /bt_mission_planner/task_responses
   │  Message: {request_id, STATUS_ACCEPTED}
   │
   ▼
6. BehaviorTree calls task action
   │
   │  Action: /task_manager/explore_task
   │  Goal: {boundary, timeout, coverage_threshold}
   │
   ▼
7. Task Manager receives action request
   │
   ├─> Looks up executor (FrontierExplorer)
   ├─> Checks if executor is ACTIVE
   └─> If SLEEPING, calls configure() + activate()
   │
   ▼
8. Task Manager forwards to executor's action server
   │
   │  Action: /frontier_explorer/execute_explore
   │  Goal: {boundary, timeout, coverage_threshold}
   │
   ▼
9. Executor (standalone node) executes task
   │
   ├─> Subscribes to /map, /robot_pose
   ├─> Detects frontiers
   ├─> Plans paths to frontiers
   ├─> Publishes cmd_vel or goals to nav stack
   └─> Sends periodic feedback
   │
   ▼
10. Feedback propagates back
    │
    │  Executor → Task Manager → BT → Panel
    │  
    │  BehaviorTree publishes TaskResponse:
    │    {request_id, STATUS_EXECUTING, progress=0.45}
    │
    ▼
11. Panel updates UI
    │
    ├─> Updates progress bar (45%)
    ├─> Updates status label ("Executing: 45% coverage")
    └─> Keeps Cancel button enabled
    │
    ▼
12. Task completes
    │
    │  Executor returns Result:
    │    {success=true, final_coverage=0.92, frontiers=18}
    │
    ▼
13. Result propagates back
    │
    │  Executor → Task Manager → BT → Panel
    │  
    │  BehaviorTree publishes TaskResponse:
    │    {request_id, STATUS_SUCCEEDED, progress=1.0}
    │
    ▼
14. Panel shows completion
    │
    ├─> Progress bar at 100%
    ├─> Status: "Task completed: 92% coverage"
    ├─> Re-enables Dispatch button
    └─> Disables Cancel button

6.2 Health Monitoring Flow

┌──────────────────────────────────────────────────────┐
│  Task Manager (periodic timer @ 0.5 Hz)              │
└────────────┬─────────────────────────────────────────┘
             │
             │ For each executor:
             ├──> Call executor->checkHealth()
             │
             ▼
┌──────────────────────────────────────────────────────┐
│  Executor (e.g., FrontierExplorer)                   │
│                                                      │
│  checkHealth() {                                     │
│    For each required topic:                          │
│      - Check if topic /map exists                    │
│      - Check if topic /robot_pose exists             │
│                                                      │
│    For each required frame:                          │
│      - Check if TF frame 'map' is published          │
│                                                      │
│    For each optional topic:                          │
│      - Check availability (warnings only)            │
│                                                      │
│    Return ExecutorStatus {                           │
│      health: HEALTHY/DEGRADED/UNAVAILABLE            │
│      missing_dependencies: ["name - description"]    │
│      warnings: ["Optional: name - description"]      │
│    }                                                 │
│  }                                                   │
└────────────┬─────────────────────────────────────────┘
             │
             │ Aggregate all executor statuses
             ▼
┌──────────────────────────────────────────────────────┐
│  Task Manager                                        │
│                                                      │
│  Publish ExecutorStatusArray {                       │
│    statuses: [                                       │
│      {name: "FrontierExplorer", health: HEALTHY},    │
│      {name: "RandomWalk", health: UNAVAILABLE,       │
│       missing_deps: ["/map - Occupancy grid"]},      │
│      ...                                             │
│    ]                                                 │
│  }                                                   │
└────────────┬─────────────────────────────────────────┘
             │
             │ Topic: /task_manager/executor_status
             ▼
┌──────────────────────────────────────────────────────┐
│  Rviz Panel                                          │
│                                                      │
│  Update executor dropdown:                           │
│    - ✓ FrontierExplorer (enabled)                   │
│    - ✗ RandomWalkExplorer (disabled, tooltip shows  │
│         "Missing: /map - Occupancy grid")            │
└──────────────────────────────────────────────────────┘

6.3 Lifecycle State Transitions

                TASK MANAGER
                     │
  ┌──────────────────┼──────────────────┐
  │                  │                  │
  │    Discovery     │    First Use     │    Idle Timeout
  │                  │                  │
  ▼                  ▼                  ▼

EXECUTOR: UNCONFIGURED → INACTIVE → ACTIVE → INACTIVE
          (Sleeping)     (Ready)    (Running)  (Ready)
          
Resources:  Minimal       Low        Full       Low
            - Node exists - Subs     - Processing - Subs exist
            - No subs     created    callbacks   but inactive
            - No timers   - Inactive - Action    - No processing
            - No action   - Action   server      - Action server
              server      server     active        inactive
                          created

---

7. Implementation Details

7.1 Package Structure

task_framework/
├── task_framework_msgs/           # Message/action/service definitions
│   ├── action/
│   │   ├── Explore.action
│   │   ├── Navigate.action
│   │   ├── Patrol.action
│   │   ├── ObjectSearch.action
│   │   └── Coverage3D.action
│   ├── msg/
│   │   ├── TaskRequest.msg
│   │   ├── TaskResponse.msg
│   │   ├── Dependency.msg
│   │   ├── ExecutorCapabilities.msg
│   │   ├── ExecutorStatus.msg
│   │   └── ExecutorStatusArray.msg
│   ├── srv/
│   │   ├── ListExecutors.srv
│   │   └── CheckExecutorHealth.srv
│   ├── CMakeLists.txt
│   └── package.xml
│
├── task_framework_core/           # Core framework
│   ├── include/task_framework/
│   │   ├── base_task_executor.hpp
│   │   ├── task_manager.hpp
│   │   └── executor_capabilities.hpp
│   ├── src/
│   │   ├── base_task_executor.cpp
│   │   ├── task_manager_node.cpp
│   │   └── dependency_checker.cpp
│   ├── CMakeLists.txt
│   └── package.xml
│
├── task_framework_bt/             # BehaviorTree integration
│   ├── include/task_framework/bt_nodes/
│   │   ├── wait_for_task_request.hpp
│   │   ├── explore_action.hpp
│   │   ├── navigate_action.hpp
│   │   ├── executor_available_condition.hpp
│   │   ├── battery_level_condition.hpp
│   │   └── select_executor_action.hpp
│   ├── src/
│   │   ├── bt_mission_planner_node.cpp
│   │   ├── bt_node_factory.cpp
│   │   └── bt_nodes/
│   │       ├── wait_for_task_request.cpp
│   │       ├── explore_action.cpp
│   │       └── ...
│   ├── behavior_trees/
│   │   ├── reactive_exploration_mission.xml
│   │   ├── wildfire_response_mission.xml
│   │   └── inspection_mission.xml
│   ├── CMakeLists.txt
│   └── package.xml
│
├── task_framework_rviz/           # Rviz panels
│   ├── src/
│   │   ├── base_task_panel.cpp
│   │   ├── explore_panel.cpp
│   │   ├── navigate_panel.cpp
│   │   └── object_search_panel.cpp
│   ├── icons/
│   ├── plugin_description.xml
│   ├── CMakeLists.txt
│   └── package.xml
│
├── exploration_executors/         # Exploration implementations
│   ├── include/
│   │   ├── frontier_explorer.hpp
│   │   └── random_walk_explorer.hpp
│   ├── src/
│   │   ├── frontier_explorer.cpp
│   │   ├── frontier_explorer_node.cpp  # Standalone node executable
│   │   ├── random_walk_explorer.cpp
│   │   └── random_walk_explorer_node.cpp
│   ├── plugins.xml
│   ├── CMakeLists.txt
│   └── package.xml
│
├── navigation_executors/          # Navigation implementations
│   ├── include/
│   │   ├── astar_navigator.hpp
│   │   └── rrt_navigator.hpp
│   ├── src/
│   │   ├── astar_navigator.cpp
│   │   ├── astar_navigator_node.cpp    # Standalone node executable
│   │   ├── rrt_navigator.cpp
│   │   └── rrt_navigator_node.cpp
│   ├── plugins.xml
│   ├── CMakeLists.txt
│   └── package.xml
│
└── task_framework_bringup/        # Launch files and configs
    ├── launch/
    │   ├── task_framework.launch.py
    │   ├── drone_mission.launch.py
    │   └── ground_robot_mission.launch.py
    ├── config/
    │   ├── task_manager.yaml
    │   ├── bt_mission_planner.yaml
    │   └── executors/
    │       ├── frontier_explorer.yaml
    │       └── astar_navigator.yaml
    └── package.xml

7.2 Build Configuration

7.2.1 task_framework_core CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(task_framework_core)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(pluginlib REQUIRED)
find_package(task_framework_msgs REQUIRED)

# Base executor library
add_library(base_task_executor SHARED
  src/base_task_executor.cpp
  src/dependency_checker.cpp
)

target_include_directories(base_task_executor PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)

ament_target_dependencies(base_task_executor
  rclcpp
  rclcpp_lifecycle
  rclcpp_action
  task_framework_msgs
)

# Task manager node
add_executable(task_manager_node
  src/task_manager_node.cpp
)

target_link_libraries(task_manager_node
  base_task_executor
)

ament_target_dependencies(task_manager_node
  rclcpp
  rclcpp_lifecycle
  rclcpp_action
  pluginlib
  task_framework_msgs
)

# Install
install(TARGETS base_task_executor
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

install(TARGETS task_manager_node
  DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY include/
  DESTINATION include
)

ament_export_include_directories(include)
ament_export_libraries(base_task_executor)
ament_export_dependencies(
  rclcpp
  rclcpp_lifecycle
  rclcpp_action
  task_framework_msgs
)

ament_package()

7.2.2 exploration_executors CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(exploration_executors)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(task_framework_core REQUIRED)
find_package(pluginlib REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)

# Frontier Explorer plugin library
add_library(frontier_explorer SHARED
  src/frontier_explorer.cpp
)

target_include_directories(frontier_explorer PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)

ament_target_dependencies(frontier_explorer
  rclcpp
  rclcpp_action
  task_framework_core
  pluginlib
  nav_msgs
  geometry_msgs
)

# Frontier Explorer standalone node
add_executable(frontier_explorer_node
  src/frontier_explorer_node.cpp
)

target_link_libraries(frontier_explorer_node
  frontier_explorer
)

# Random Walk Explorer plugin library
add_library(random_walk_explorer SHARED
  src/random_walk_explorer.cpp
)

ament_target_dependencies(random_walk_explorer
  rclcpp
  rclcpp_action
  task_framework_core
  pluginlib
  nav_msgs
  geometry_msgs
)

# Random Walk Explorer standalone node
add_executable(random_walk_explorer_node
  src/random_walk_explorer_node.cpp
)

target_link_libraries(random_walk_explorer_node
  random_walk_explorer
)

# Mark as plugins
pluginlib_export_plugin_description_file(task_framework_core plugins.xml)

# Install libraries
install(TARGETS frontier_explorer random_walk_explorer
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

# Install executables
install(TARGETS frontier_explorer_node random_walk_explorer_node
  DESTINATION lib/${PROJECT_NAME}
)

install(FILES plugins.xml
  DESTINATION share/${PROJECT_NAME}
)

ament_package()

7.2.3 Standalone Node Executable

// src/frontier_explorer_node.cpp
#include "frontier_explorer.hpp"
#include <rclcpp/rclcpp.hpp>

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  
  auto executor = std::make_shared<explorers::FrontierExplorer>();
  
  // Configure and activate
  executor->configure();
  executor->activate();
  
  rclcpp::spin(executor->get_node_base_interface());
  
  rclcpp::shutdown();
  return 0;
}

7.2.4 plugins.xml

<library path="frontier_explorer">
  <class type="explorers::FrontierExplorer" 
         base_class_type="task_framework::BaseTaskExecutor">
    <description>
      Frontier-based exploration using occupancy grid analysis.
      Can be run standalone: ros2 run exploration_executors frontier_explorer_node
      Action server: ~/execute_explore
    </description>
  </class>
</library>

<library path="random_walk_explorer">
  <class type="explorers::RandomWalkExplorer" 
         base_class_type="task_framework::BaseTaskExecutor">
    <description>
      Random walk exploration for simple coverage.
      Can be run standalone: ros2 run exploration_executors random_walk_explorer_node
      Action server: ~/execute_explore
    </description>
  </class>
</library>

7.3 Launch File

File: task_framework_bringup/launch/task_framework.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    # Get package directories
    bringup_dir = get_package_share_directory('task_framework_bringup')
    bt_dir = get_package_share_directory('task_framework_bt')
    
    # Declare arguments
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    bt_tree_file = LaunchConfiguration('bt_tree_file', 
                                       default='reactive_exploration_mission.xml')
    
    return LaunchDescription([
        # Declare launch arguments
        DeclareLaunchArgument('use_sim_time'),
        DeclareLaunchArgument('bt_tree_file'),
        
        # Task Manager
        Node(
            package='task_framework_core',
            executable='task_manager_node',
            name='task_manager',
            output='screen',
            parameters=[
                os.path.join(bringup_dir, 'config', 'task_manager.yaml'),
                {'use_sim_time': use_sim_time}
            ]
        ),
        
        # BehaviorTree Mission Planner
        Node(
            package='task_framework_bt',
            executable='bt_mission_planner',
            name='bt_mission_planner',
            output='screen',
            parameters=[
                os.path.join(bringup_dir, 'config', 'bt_mission_planner.yaml'),
                {
                    'use_sim_time': use_sim_time,
                    'tree_file': os.path.join(bt_dir, 'behavior_trees', bt_tree_file)
                }
            ]
        ),
        
        # Note: Executor nodes are launched by Task Manager via pluginlib
        # For standalone testing, executors can be launched separately:
        # ros2 run exploration_executors frontier_explorer_node
    ])

7.4 Configuration Files

File: config/task_manager.yaml

task_manager:
  ros__parameters:
    # Executor management
    executor_mode: "lazy_activation"  # Options: all_active, lazy_activation, on_demand
    idle_timeout_minutes: 5
    auto_cleanup_enabled: true
    
    # Health monitoring
    health_check_rate: 0.5  # Hz
    health_check_timeout: 1.0  # seconds
    
    # Action server settings
    action_server_timeout: 300.0  # seconds
    
    # Logging
    enable_detailed_logging: false
    log_executor_transitions: true

File: config/bt_mission_planner.yaml

bt_mission_planner:
  ros__parameters:
    # BehaviorTree settings
    tick_rate: 10.0  # Hz
    tree_file: "reactive_exploration_mission.xml"
    
    # Groot visualization
    enable_groot: true
    groot_publisher_port: 1666
    groot_server_port: 1667
    
    # Safety limits
    battery_emergency_threshold: 0.15
    battery_warning_threshold: 0.25
    gps_min_satellites: 6
    max_temperature_celsius: 70
    
    # Blackboard defaults
    home_pose:
      position:
        x: 0.0
        y: 0.0
        z: 0.0
      orientation:
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0

---

8. Deployment

8.1 System Requirements

Hardware:

• Minimum: Dual-core CPU, 4GB RAM
• Recommended: Quad-core CPU, 8GB RAM
• For drones: Consider power constraints

Software:

• Ubuntu 22.04 (Jammy)
• ROS2 Humble or Iron
• BehaviorTree.CPP 4.x
• Qt5
• C++17 compiler

8.2 Installation

# Create workspace
mkdir -p ~/task_framework_ws/src
cd ~/task_framework_ws/src

# Clone repositories
git clone https://github.com/your-org/task_framework.git

# Install dependencies
cd ~/task_framework_ws
rosdep install --from-paths src --ignore-src -r -y

# Build
colcon build --symlink-install

# Source
source install/setup.bash

8.3 Launch Configurations

8.3.1 Development Mode (Test Executor Standalone)

# Test executor independently
ros2 run exploration_executors frontier_explorer_node

# In another terminal, send test goal
ros2 action send_goal /frontier_explorer/execute_explore \
  task_framework_msgs/action/Explore \
  "{boundary: 'POLYGON((0 0, 10 0, 10 10, 0 10))', timeout: 300.0, coverage_threshold: 0.9}"

8.3.2 Full System with GUI

# Launch task framework
ros2 launch task_framework_bringup task_framework.launch.py \
  use_sim_time:=true \
  bt_tree_file:=reactive_exploration_mission.xml

# Launch Groot for BT visualization
ros2 run groot Groot

# Launch Rviz with panels
rviz2 -d $(ros2 pkg prefix task_framework_rviz)/share/task_framework_rviz/config/task_panels.rviz

8.3.3 Simulation

# Launch Gazebo simulation
ros2 launch your_robot_gazebo simulation.launch.py

# Launch task framework
ros2 launch task_framework_bringup task_framework.launch.py \
  use_sim_time:=true

# Launch Rviz
rviz2 -d task_panels.rviz

8.3.4 Real Robot (Drone)

# Launch PX4 interface
ros2 launch px4_interface mavlink_bridge.launch.py

# Launch localization and mapping
ros2 launch your_drone_stack vio_and_slam.launch.py

# Launch task framework
ros2 launch task_framework_bringup drone_mission.launch.py

# On ground station, launch Rviz
rviz2 -d drone_mission.rviz

8.4 Verifying Installation

# Check Task Manager is running
ros2 node list | grep task_manager

# Check executors discovered
ros2 service call /task_manager/list_executors \
  task_framework_msgs/srv/ListExecutors "{task_type: 'TaskExplore'}"

# Monitor executor health
ros2 topic echo /task_manager/executor_status

# Check BehaviorTree is loaded
ros2 node list | grep bt_mission_planner

# Test standalone executor
ros2 run exploration_executors frontier_explorer_node &
ros2 action list | grep execute_explore

---

9. Testing Strategy

9.1 Unit Tests

9.1.1 Dependency Checking

// test/test_dependency_checker.cpp
TEST(DependencyChecker, TopicAvailability) {
  auto executor = std::make_shared<FrontierExplorer>();
  executor->configure();
  
  // Should be unavailable before map is published
  auto status = executor->checkHealth();
  EXPECT_EQ(status.health, ExecutorHealth::UNAVAILABLE);
  EXPECT_GT(status.missing_dependencies.size(), 0);
  
  // Start publishing map
  auto map_pub = node->create_publisher<nav_msgs::msg::OccupancyGrid>("/map", 10);
  // ... publish a message ...
  
  // Should become healthy after map is available
  status = executor->checkHealth();
  EXPECT_EQ(status.health, ExecutorHealth::HEALTHY);
}

9.1.2 Health Status

TEST(BaseTaskExecutor, HealthStatus) {
  auto executor = std::make_shared<MockExplorer>();
  
  // Initially unconfigured
  auto status = executor->checkHealth();
  EXPECT_EQ(status.health, ExecutorHealth::UNCONFIGURED);
  
  // After configuration
  executor->configure();
  status = executor->checkHealth();
  EXPECT_EQ(status.health, ExecutorHealth::UNAVAILABLE);
  EXPECT_GT(status.missing_dependencies.size(), 0);
  
  // After dependencies are met
  // ... set up mock topics ...
  status = executor->checkHealth();
  EXPECT_EQ(status.health, ExecutorHealth::HEALTHY);
}

9.2 Integration Tests

9.2.1 Standalone Executor Test

TEST(IntegrationTest, StandaloneExecutorOperation) {
  // Launch executor as standalone node
  auto executor = std::make_shared<FrontierExplorer>();
  executor->configure();
  executor->activate();
  
  // Create action client
  auto client = rclcpp_action::create_client<ExploreAction>(
    node, "/frontier_explorer/execute_explore");
  
  // Send goal
  ExploreAction::Goal goal;
  goal.boundary = "POLYGON((0 0, 10 0, 10 10, 0 10))";
  goal.timeout = 60.0;
  goal.coverage_threshold = 0.8;
  
  auto future = client->async_send_goal(goal);
  
  // Verify action accepted
  ASSERT_EQ(future.wait_for(std::chrono::seconds(5)), 
            std::future_status::ready);
}

9.2.2 Full Task Dispatch Pipeline

TEST(IntegrationTest, FullTaskDispatch) {
  // 1. Start Task Manager
  auto task_manager = std::make_shared<TaskManager>();
  
  // 2. Start BT Planner
  auto bt_planner = std::make_shared<BehaviorTreeMissionPlanner>();
  
  // 3. Publish TaskRequest
  TaskRequest req;
  req.request_id = "test_001";
  req.task_type = "TaskExplore";
  req.parameters_json = R"({
    "boundary": "POLYGON((0 0, 10 0, 10 10, 0 10, 0 0))",
    "timeout": 60.0,
    "coverage_threshold": 0.8
  })";
  
  request_pub_->publish(req);
  
  // 4. Wait for acceptance
  // ... verify TaskResponse with STATUS_ACCEPTED ...
  
  // 5. Wait for completion
  // ... verify TaskResponse with STATUS_SUCCEEDED ...
}

9.3 Mock Executors

For testing GUI and BT without real robot:

// test/mock_executors/instant_explorer.cpp
class InstantExplorer : public BaseTaskExecutor {
public:
  InstantExplorer() : BaseTaskExecutor("instant_explorer") {}
  
  ExecutorCapabilities getCapabilities() const override {
    ExecutorCapabilities caps;
    caps.executor_name = "InstantExplorer";
    caps.compatible_task_type = "TaskExplore";
    caps.description = "Test executor that completes instantly";
    // No dependencies!
    return caps;
  }
  
protected:
  void handleAccepted(GoalHandle goal_handle) {
    // Immediately succeed
    auto result = std::make_shared<ExploreAction::Result>();
    result->success = true;
    result->final_coverage = 0.95;
    result->frontiers_explored = 10;
    goal_handle->succeed(result);
  }
};

---

10. Future Extensions

10.1 Multi-Robot Coordination

• Fleet Manager: Coordinate tasks across multiple robots
• Task Allocation: Intelligent assignment based on robot capabilities
• Shared Executors: Allow executors to manage multiple robots

10.2 Advanced Task Types

• TaskOpenLanguage: Natural language task specification
• TaskCollaborate: Multi-robot collaborative tasks
• TaskInspect: Infrastructure inspection with waypoint generation

---

11. Appendices

11.1 Glossary

Term	Definition
Task	High-level robot capability (explore, navigate, search)
Executor	Implementation of a task strategy (plugin + standalone node)
Task Manager	Orchestrates executor discovery, lifecycle, routing
BT	BehaviorTree - reactive mission planning framework
Lifecycle Node	ROS2 node with managed state transitions
Pluginlib	ROS2 library for dynamic class loading
Dependency	Resource required by executor (topic, service, TF frame)
Health Check	Validation that executor dependencies are available
Lazy Activation	Deferring resource allocation until first use
Standalone Node	Executor can run independently as a ROS2 node

11.2 Common Executor Patterns

Pattern 1: Map-Based Executor

class MapBasedExecutor : public BaseTaskExecutor {
  ExecutorCapabilities getCapabilities() const override {
    ExecutorCapabilities caps;
    caps.executor_name = "MapBasedExecutor";
    caps.compatible_task_type = "TaskExplore";
    
    caps.required_topics = {
      {"/map", "Occupancy grid for planning"},
      {"/robot_pose", "Robot localization"}
    };
    
    caps.required_frames = {
      {"map", "Global reference frame"}
    };
    
    return caps;
  }
};

Pattern 2: Vision-Based Executor

class VisionBasedExecutor : public BaseTaskExecutor {
  ExecutorCapabilities getCapabilities() const override {
    ExecutorCapabilities caps;
    caps.executor_name = "VisionBasedExecutor";
    caps.compatible_task_type = "TaskObjectSearch";
    
    caps.required_topics = {
      {"/camera/image", "Camera feed for detection"},
      {"/detections", "Object detection results"}
    };
    
    caps.optional_services = {
      {"/object_detector", "On-demand detection service"}
    };
    
    return caps;
  }
};

Pattern 3: Pure Planning Executor

class PlanningExecutor : public BaseTaskExecutor {
  ExecutorCapabilities getCapabilities() const override {
    ExecutorCapabilities caps;
    caps.executor_name = "PlanningExecutor";
    caps.compatible_task_type = "TaskNavigate";
    
    caps.required_services = {
      {"/global_costmap", "Global planning costmap"}
    };
    
    // No sensor dependencies
    
    return caps;
  }
};

11.3 Troubleshooting

Issue	Diagnosis	Solution
Executor not discovered	Check plugins.xml path	Verify <export> in package.xml
Executor always unavailable	Check dependency topics	Use ros2 topic list to verify
Task not dispatched	Check BT acceptance	View Groot to see BT state
Panel dropdown empty	Check status publishing	ros2 topic echo /task_manager/executor_status
Slow activation	Too many executors	Use on-demand loading mode
Memory leak	Executors not cleaned up	Enable auto-cleanup, reduce idle timeout
Standalone executor won't start	Missing dependencies	Check executor's getCapabilities()
Action server not found	Wrong namespace	Check ~/execute_* action name

11.4 Performance Tuning

Reduce Latency:

# Faster health checks, faster activation
health_check_rate: 1.0  # Default: 0.5 Hz

Reduce Memory:

# More aggressive cleanup
idle_timeout_minutes: 2  # Default: 5
auto_cleanup_enabled: true

Increase Reliability:

# Keep executors always ready
executor_mode: "all_active"  # No lazy activation

11.5 Design Decisions Log

Decision	Rationale
Keep executor action servers	Enables standalone development, testing, and deployment flexibility
Simplify dependency declaration	Reduces boilerplate by 70%, clearer intent (required vs optional)
Use lifecycle nodes	Resource efficiency with lazy activation
BehaviorTree integration	Provides retry, fallback, and safety logic at mission level
Pluginlib for discovery	Dynamic loading without recompiling core framework

11.6 References

• ROS2 Lifecycle Nodes
• Pluginlib Tutorial
• BehaviorTree.CPP Documentation
• ROS2 Actions
• Rviz Plugin Tutorial

---

Document Revision History

Version	Date	Author	Changes
1.0	Jan 2026	Andrew	Initial design document
1.1	Jan 2026	Andrew	Simplified dependency declaration, clarified executor action server design rationale

[andrewjong]

Perhaps we don't want to stick with RViz. Foxglove may be better.