# Hướng dẫn triển khai SimpleActionServer trong MoveBase

## Tổng quan

Tài liệu này hướng dẫn cách tích hợp `SimpleActionServer` vào class `MoveBase` hiện có.

## Bước 1: Thêm SimpleActionServer vào MoveBase class

### Trong `move_base.h`:

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

namespace move_base
{
  class MoveBase : public robot::move_base_core::BaseNavigation
  {
  private:
    // Thêm member variable
    std::shared_ptr<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>> as_;
    
    // Thêm method declaration
    void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal);
  };
}
```

## Bước 2: Khởi tạo trong constructor hoặc initialize()

### Trong `move_base.cpp` constructor hoặc `initialize()`:

```cpp
void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
{
  // ... existing initialization code ...
  
  // Khởi tạo action server
  as_ = std::make_shared<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>>(
    "move_base",
    std::bind(&MoveBase::executeCb, this, std::placeholders::_1),
    true  // auto_start
  );
  
  // Setup callbacks (optional)
  setupActionServerCallbacks();
  
  robot::log_info("[MoveBase] Action server initialized");
}
```

## Bước 3: Setup callbacks (nếu cần publish messages)

```cpp
void move_base::MoveBase::setupActionServerCallbacks()
{
  if (!as_)
    return;
  
  // Set callback để publish result
  // Customize để gửi qua hệ thống communication của bạn
  as_->setResultCallback([](const auto& result) {
    // TODO: Gửi result qua hệ thống communication của bạn
    // your_communication_system.send(result);
    robot::log_info("Result: status=%d", result->status.status);
  });
  
  // Set callback để publish feedback
  as_->setFeedbackCallback([](const auto& feedback) {
    // TODO: Gửi feedback qua hệ thống communication của bạn
    // your_communication_system.send(feedback);
  });
  
  // Set callback để publish status
  as_->setStatusCallback([](const auto& status) {
    // TODO: Gửi status qua hệ thống communication của bạn
    // your_communication_system.send(status);
  });
}
```

## Bước 4: Triển khai executeCb()

### Trong `move_base.cpp`:

```cpp
void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal)
{
  robot::log_info("[MoveBase] Execute callback: x=%.2f, y=%.2f",
                 goal->target_pose.pose.position.x,
                 goal->target_pose.pose.position.y);
  
  // Validate quaternion
  if (!isQuaternionValid(goal->target_pose.pose.orientation))
  {
    robot::log_error("[MoveBase] Invalid quaternion in goal");
    as_->setAborted(robot_move_base_msgs::MoveBaseResult(),
                   "Invalid quaternion in goal");
    return;
  }
  
  // Convert goal to global frame
  robot_geometry_msgs::PoseStamped target_pose = goalToGlobalFrame(goal->target_pose);
  
  // Send goal to move_base
  bool success = moveTo(target_pose, 0.2, 0.2);
  
  if (!success)
  {
    robot::log_error("[MoveBase] Failed to send goal");
    as_->setAborted(robot_move_base_msgs::MoveBaseResult(),
                   "Failed to send goal to move_base");
    return;
  }
  
  // Monitor execution
  robot::Rate rate(10.0);
  robot::Time last_feedback_time = robot::Time::now();
  
  while (as_->isActive() && !as_->isPreemptRequested())
  {
    // Get feedback
    robot::move_base_core::NavFeedback* feedback = getFeedback();
    
    if (feedback)
    {
      // Check if goal reached
      if (feedback->goal_checked && 
          feedback->navigation_state == robot::move_base_core::State::SUCCEEDED)
      {
        as_->setSucceeded(robot_move_base_msgs::MoveBaseResult(),
                         "Goal reached");
        return;
      }
      
      // Check for errors
      if (feedback->navigation_state == robot::move_base_core::State::ABORTED)
      {
        as_->setAborted(robot_move_base_msgs::MoveBaseResult(),
                       feedback->feed_back_str);
        return;
      }
      
      // Publish feedback periodically
      robot::Time now = robot::Time::now();
      if ((now - last_feedback_time).toSec() >= 0.5)
      {
        robot_move_base_msgs::MoveBaseFeedback fb;
        fb.base_position.x = feedback->current_pose.x;
        fb.base_position.y = feedback->current_pose.y;
        fb.base_position.theta = feedback->current_pose.theta;
        fb.base_position.header.stamp = robot::Time::now();
        fb.base_position.header.frame_id = global_frame_;
        
        as_->publishFeedback(fb);
        last_feedback_time = now;
      }
    }
    
    // Check for preemption
    if (as_->isPreemptRequested())
    {
      cancel();
      as_->setPreempted(robot_move_base_msgs::MoveBaseResult(),
                       "Preempted");
      return;
    }
    
    rate.sleep();
  }
  
  // Handle unexpected exit
  if (as_->isPreemptRequested())
  {
    cancel();
    as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Preempted");
  }
  else
  {
    as_->setAborted(robot_move_base_msgs::MoveBaseResult(),
                   "Execution ended unexpectedly");
  }
}
```

## Bước 5: Xử lý goal và cancel messages từ bên ngoài

Nhận goal và cancel messages từ hệ thống communication của bạn:

```cpp
// Trong callback nhận goal từ hệ thống communication của bạn
void move_base::MoveBase::goalCallback(
    const robot_move_base_msgs::MoveBaseActionGoalConstPtr& msg)
{
  if (as_)
  {
    as_->processGoal(msg);
  }
}

// Trong callback nhận cancel từ hệ thống communication của bạn
void move_base::MoveBase::cancelCallback(
    const robot_actionlib_msgs::GoalIDConstPtr& msg)
{
  if (as_)
  {
    as_->processCancel(msg);
  }
}

// Ví dụ: Đăng ký với hệ thống communication của bạn
// your_communication_system.onGoal(
//     std::bind(&MoveBase::goalCallback, this, std::placeholders::_1));
// your_communication_system.onCancel(
//     std::bind(&MoveBase::cancelCallback, this, std::placeholders::_1));
```

## Bước 6: Cleanup trong destructor

```cpp
move_base::MoveBase::~MoveBase()
{
  // ... existing cleanup ...
  
  // Stop action server
  if (as_)
  {
    as_->stop();
    as_.reset();
  }
}
```

## Ví dụ hoàn chỉnh

Xem các file trong thư mục `examples/`:
- `simple_usage_example.cpp` - Ví dụ đơn giản nhất
- `simple_action_server_example.cpp` - Ví dụ đầy đủ
- `move_base_with_action_server_example.cpp` - Ví dụ tích hợp vào MoveBase

## Lưu ý

1. **Thread Safety**: `executeCb` chạy trong thread riêng, đảm bảo thread-safe khi truy cập các member variables
2. **State Management**: Action server tự động quản lý state, không cần can thiệp thủ công
3. **Feedback Rate**: Điều chỉnh rate publish feedback phù hợp (khuyến nghị 0.5-1.0 giây)
4. **Error Handling**: Luôn kiểm tra và xử lý các trường hợp lỗi

## Testing

Để test action server:

```cpp
// Tạo goal message
robot_move_base_msgs::MoveBaseActionGoalPtr goal = 
    boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
goal->goal_id.id = "test_goal_1";
goal->goal.target_pose.pose.position.x = 5.0;
goal->goal.target_pose.pose.position.y = 3.0;

// Gửi goal
move_base_ptr->getActionServer()->processGoal(goal);

// Kiểm tra status
auto status = move_base_ptr->getActionServer()->getCurrentStatus();
```

