# SimpleActionServer - Hướng dẫn sử dụng

## Tổng quan

`SimpleActionServer` là một thư viện tương tự ROS actionlib::SimpleActionServer nhưng không phụ thuộc vào ROS. Nó cung cấp interface đơn giản để triển khai action server với các tính năng:

- Quản lý goal requests
- Hỗ trợ preemption (hủy goal)
- Publish feedback và result thông qua callbacks
- Thread-safe và thread-safe state management
- Tương thích với framework robot hiện có

## Cấu trúc Files

- `simple_action_server.h` - Header file với class declaration
- `simple_action_server.cpp` - Implementation file với template code
- `simple_action_server_example.cpp` - Ví dụ triển khai đầy đủ

## Ví dụ sử dụng cơ bản

### 1. Khởi tạo SimpleActionServer

```cpp
#include <move_base/simple_action_server.h>

// Tạo action server với execute callback
auto action_server = std::make_shared<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>>(
    "move_base",
    [this](const auto& goal) { this->executeCb(goal); },
    true  // auto_start
);
```

### 2. Setup callbacks để publish messages

```cpp
// Set callback để publish result
action_server->setResultCallback([](const auto& result) {
    // Publish result message qua ROS hoặc hệ thống khác
    result_pub_.publish(*result);
});

// Set callback để publish feedback
action_server->setFeedbackCallback([](const auto& feedback) {
    // Publish feedback message
    feedback_pub_.publish(*feedback);
});

// Set callback để publish status
action_server->setStatusCallback([](const auto& status) {
    // Publish status message
    status_pub_.publish(*status);
});
```

### 3. Xử lý goal messages từ bên ngoài

```cpp
// Trong ROS subscriber callback hoặc nơi nhận goal
void goalCallback(const robot_move_base_msgs::MoveBaseActionGoalConstPtr& msg)
{
    action_server->processGoal(msg);
}

// Trong cancel callback
void cancelCallback(const robot_actionlib_msgs::GoalIDConstPtr& msg)
{
    action_server->processCancel(msg);
}
```

### 4. Triển khai execute callback

```cpp
void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal)
{
    robot::log_info("Received goal: x=%.2f, y=%.2f", 
                   goal->target_pose.pose.position.x,
                   goal->target_pose.pose.position.y);
    
    // Gửi goal đến move_base
    bool success = move_base_ptr_->moveTo(goal->target_pose, 0.2, 0.2);
    
    if (!success)
    {
        action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(),
                                   "Failed to send goal");
        return;
    }
    
    // Monitor execution
    robot::Rate rate(10.0);
    while (action_server_->isActive() && !action_server_->isPreemptRequested())
    {
        // Kiểm tra trạng thái
        auto* feedback = move_base_ptr_->getFeedback();
        if (feedback->goal_checked && 
            feedback->navigation_state == robot::move_base_core::State::SUCCEEDED)
        {
            action_server_->setSucceeded(robot_move_base_msgs::MoveBaseResult(),
                                        "Goal reached");
            return;
        }
        
        // Publish feedback
        robot_move_base_msgs::MoveBaseFeedback fb;
        fb.base_position.x = feedback->current_pose.x;
        fb.base_position.y = feedback->current_pose.y;
        action_server_->publishFeedback(fb);
        
        rate.sleep();
    }
    
    // Xử lý preemption
    if (action_server_->isPreemptRequested())
    {
        move_base_ptr_->cancel();
        action_server_->setPreempted(robot_move_base_msgs::MoveBaseResult(),
                                    "Preempted");
    }
}
```

## Tích hợp vào MoveBase

### Cách 1: Thêm vào MoveBase class

```cpp
// Trong move_base.h
#include <move_base/simple_action_server.h>

class MoveBase : public robot::move_base_core::BaseNavigation
{
private:
    std::shared_ptr<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>> as_;
    
    void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal);
};
```

```cpp
// Trong move_base.cpp constructor
MoveBase::MoveBase(robot::TFListenerPtr tf)
{
    // ... existing initialization ...
    
    // Create action server
    as_ = std::make_shared<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>>(
        "move_base",
        std::bind(&MoveBase::executeCb, this, std::placeholders::_1),
        true
    );
    
    // Setup callbacks
    as_->setResultCallback([this](const auto& result) {
        // Publish result if needed
    });
}
```

### Cách 2: Sử dụng trong wrapper class

Xem file `simple_action_server_example.cpp` để xem ví dụ đầy đủ.

## API Reference

### Constructor
```cpp
SimpleActionServer(const std::string& name,
                   ExecuteCallback execute_callback,
                   bool auto_start = true)
```

### Main Methods
- `void processGoal(const ActionGoalConstPtr& goal_msg)` - Xử lý goal message
- `void processCancel(const GoalIDConstPtr& cancel_msg)` - Xử lý cancel message
- `GoalConstPtr acceptNewGoal()` - Chấp nhận goal mới
- `bool isNewGoalAvailable()` - Kiểm tra có goal mới
- `bool isPreemptRequested()` - Kiểm tra preemption
- `void setSucceeded(const Result&, const std::string&)` - Đặt thành công
- `void setAborted(const Result&, const std::string&)` - Đặt lỗi
- `void setPreempted(const Result&, const std::string&)` - Đặt preempted
- `void publishFeedback(const Feedback&)` - Publish feedback

### Callback Setup
- `void setGoalCallback(GoalCallback)` - Callback khi nhận goal
- `void setCancelCallback(CancelCallback)` - Callback khi nhận cancel
- `void setFeedbackCallback(FeedbackCallback)` - Callback để publish feedback
- `void setResultCallback(ResultCallback)` - Callback để publish result
- `void setStatusCallback(StatusCallback)` - Callback để publish status

## Lưu ý

1. **Thread Safety**: Tất cả các phương thức đều thread-safe
2. **Callbacks**: Các callbacks được gọi trong context của thread tương ứng
3. **State Management**: Action server tự động quản lý state machine
4. **No ROS Dependency**: Không cần ROS, chỉ cần setup callbacks để publish messages

## Tích hợp với hệ thống communication khác

SimpleActionServer không phụ thuộc vào ROS. Bạn có thể tích hợp với bất kỳ hệ thống communication nào bằng cách setup callbacks:

```cpp
// Setup callbacks để publish messages qua hệ thống của bạn
action_server->setResultCallback([](const auto& result) {
    // Gửi result qua hệ thống communication của bạn
    // your_communication_system.send(result);
});

action_server->setFeedbackCallback([](const auto& feedback) {
    // Gửi feedback qua hệ thống communication của bạn
    // your_communication_system.send(feedback);
});

// Nhận goal messages từ hệ thống của bạn
// your_communication_system.onGoal([&action_server](const auto& msg) {
//     action_server->processGoal(msg);
// });
```

