robot_cpp/INIT_USAGE.md

# robot::ok() - Hướng dẫn sử dụng

## Tổng quan

`robot::ok()` và các hàm liên quan cung cấp cơ chế quản lý vòng đời của robot system, tương tự như `ros::ok()` trong ROS. Chúng cho phép bạn kiểm tra trạng thái system và xử lý shutdown một cách graceful.

## API Reference

### `bool robot::ok()`

Kiểm tra xem robot system có đang chạy không.

- **Trả về `true`**: System đang chạy bình thường
- **Trả về `false`**: System đã shutdown hoàn toàn

### `bool robot::isShuttingDown()`

Kiểm tra xem shutdown đã được yêu cầu chưa.

- **Trả về `true`**: Shutdown đã được yêu cầu (ngay lập tức)
- **Trả về `false`**: System vẫn đang chạy

### `void robot::shutdown()`

Bắt đầu quá trình shutdown system.

### `void robot::init(int& argc, char** argv, const std::string& node_name = "", bool install_sigint_handler = true)`

Khởi tạo robot system.

- `argc`, `argv`: Command line arguments (có thể được modify)
- `node_name`: Tên node (optional, dùng cho logging)
- `install_sigint_handler`: Nếu `true`, tự động xử lý Ctrl-C (default: `true`)

### `void robot::init(const std::string& node_name = "", bool install_sigint_handler = true)`

Khởi tạo robot system không cần command line arguments.

## Cách sử dụng cơ bản

### 1. Main Loop với Rate

```cpp
#include <robot/init.h>
#include <robot/time.h>

int main(int argc, char** argv)
{
  // Initialize robot system
  robot::init(argc, argv, "my_node");
  
  robot::Rate rate(10);  // 10 Hz
  
  // Main loop
  while (robot::ok())
  {
    // Do work here
    std::cout << "Running..." << std::endl;
    
    rate.sleep();
  }
  
  return 0;
}
```

### 2. Main Loop không dùng Rate

```cpp
#include <robot/init.h>
#include <robot/time.h>

int main(int argc, char** argv)
{
  robot::init(argc, argv, "my_node");
  
  while (robot::ok())
  {
    // Do work
    robot::Duration(0.1).sleep();  // Sleep 100ms
  }
  
  return 0;
}
```

### 3. Thread Loop

```cpp
#include <robot/init.h>
#include <thread>
#include <atomic>

std::atomic<bool> stop_flag_(false);

void myThread()
{
  while (robot::ok() && !stop_flag_)
  {
    // Do work in thread
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
}

int main(int argc, char** argv)
{
  robot::init(argc, argv, "my_node");
  
  std::thread t(myThread);
  
  // Main loop
  while (robot::ok())
  {
    // Do main work
    robot::Duration(1.0).sleep();
  }
  
  stop_flag_ = true;
  t.join();
  
  return 0;
}
```

### 4. Long-running Callback với Early Exit

```cpp
#include <robot/init.h>

void processLargeData(const std::vector<int>& data)
{
  for (size_t i = 0; i < data.size(); i++)
  {
    // Check if shutdown requested
    if (robot::isShuttingDown())
    {
      std::cout << "Shutdown requested, exiting early" << std::endl;
      return;  // Exit early
    }
    
    // Process data[i]...
    processItem(data[i]);
  }
}

int main(int argc, char** argv)
{
  robot::init(argc, argv, "my_node");
  
  std::vector<int> large_data(1000000);
  // ... fill data ...
  
  while (robot::ok())
  {
    processLargeData(large_data);
    robot::Duration(1.0).sleep();
  }
  
  return 0;
}
```

### 5. Custom SIGINT Handler

```cpp
#include <robot/init.h>
#include <signal.h>
#include <robot/time.h>

void mySigintHandler(int sig)
{
  (void)sig;
  
  // Do custom cleanup
  std::cout << "Custom cleanup before shutdown..." << std::endl;
  
  // Publish stop message, save state, etc.
  
  // Call shutdown
  robot::shutdown();
}

int main(int argc, char** argv)
{
  // Initialize without SIGINT handler
  robot::init(argc, argv, "my_node", false);
  
  // Install custom handler
  signal(SIGINT, mySigintHandler);
  
  while (robot::ok())
  {
    // Do work
    robot::Duration(0.1).sleep();
  }
  
  return 0;
}
```

### 6. Nested Loops

```cpp
#include <robot/init.h>
#include <robot/time.h>

int main(int argc, char** argv)
{
  robot::init(argc, argv, "my_node");
  
  while (robot::ok())  // Outer loop
  {
    // Setup connection
    bool connected = connectToDevice();
    
    while (robot::ok() && connected)  // Inner loop
    {
      // Do work with device
      if (!deviceIsAlive())
      {
        connected = false;
        break;
      }
      
      robot::Duration(0.1).sleep();
    }
    
    // Reconnect after delay
    robot::Duration(1.0).sleep();
  }
  
  return 0;
}
```

### 7. Multiple Conditions

```cpp
#include <robot/init.h>
#include <robot/time.h>
#include <atomic>

std::atomic<bool> cancel_(false);
std::atomic<bool> connected_(false);

int main(int argc, char** argv)
{
  robot::init(argc, argv, "my_node");
  
  while (robot::ok() && !cancel_ && connected_)
  {
    // Do work only if all conditions are met
    processData();
    
    robot::Duration(0.1).sleep();
  }
  
  return 0;
}
```

## So sánh với ROS

### Tương đương với ROS:

| ROS | robot:: | Mô tả |
|-----|---------|-------|
| `ros::ok()` | `robot::ok()` | Kiểm tra system đang chạy |
| `ros::isShuttingDown()` | `robot::isShuttingDown()` | Kiểm tra shutdown được yêu cầu |
| `ros::shutdown()` | `robot::shutdown()` | Bắt đầu shutdown |
| `ros::init(argc, argv, "node")` | `robot::init(argc, argv, "node")` | Khởi tạo system |

### Ví dụ chuyển đổi từ ROS:

**ROS Code:**
```cpp
#include <ros/ros.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "my_node");
  ros::NodeHandle nh;
  ros::Rate rate(10);
  
  while (ros::ok())
  {
    ros::spinOnce();
    rate.sleep();
  }
  
  return 0;
}
```

**robot:: Code:**
```cpp
#include <robot/init.h>
#include <robot/time.h>

int main(int argc, char** argv)
{
  robot::init(argc, argv, "my_node");
  robot::Rate rate(10);
  
  while (robot::ok())
  {
    // Do work (no ros::spinOnce() equivalent needed)
    rate.sleep();
  }
  
  return 0;
}
```

## Best Practices

### ✅ Nên làm:

1. **Luôn gọi `robot::init()` trước khi dùng các hàm khác**
   ```cpp
   int main(int argc, char** argv)
   {
     robot::init(argc, argv, "my_node");
     // ... rest of code
   }
   ```

2. **Luôn kiểm tra `robot::ok()` trong vòng lặp chính**
   ```cpp
   while (robot::ok())
   {
     // Do work
   }
   ```

3. **Kiểm tra trong thread loops**
   ```cpp
   while (robot::ok() && !stop_flag_)
   {
     // Thread work
   }
   ```

4. **Dùng `robot::isShuttingDown()` trong callback dài**
   ```cpp
   if (robot::isShuttingDown()) return;
   ```

5. **Kết hợp với các điều kiện khác**
   ```cpp
   while (robot::ok() && connected && !cancel_)
   {
     // Work
   }
   ```

### ❌ Không nên:

1. **Không bỏ qua kiểm tra trong thread**
   - Thread có thể chạy mãi nếu không kiểm tra

2. **Không dùng `robot::ok()` trong callback ngắn**
   - Callback ngắn sẽ chạy xong trước khi shutdown
   - Dùng `robot::isShuttingDown()` thay vì

3. **Không gọi `robot::shutdown()` trong callback**
   - Có thể gây deadlock
   - Để shutdown tự nhiên hoặc dùng flag

## Shutdown Process

### Quy trình shutdown:

1. **SIGINT (Ctrl-C) được gửi** hoặc `robot::shutdown()` được gọi
2. **`robot::isShuttingDown()`** → `true` (ngay lập tức)
3. **`robot::ok()`** → vẫn `true` (chưa shutdown hoàn toàn)
4. **Resources được cleanup** (nếu có)
5. **`robot::ok()`** → `false` (shutdown hoàn tất)

### Timeline:

```
Time    | Event                    | ok() | isShuttingDown()
--------|--------------------------|------|------------------
T0      | System running           | true | false
T1      | Ctrl-C pressed           | true | false
T2      | shutdown() called        | true | true  ← isShuttingDown() = true
T3      | Cleanup in progress      | true | true
T4      | Cleanup complete         | false| true  ← ok() = false
```

## Ví dụ thực tế trong move_base

Bạn có thể sử dụng trong `planThread()`:

```cpp
#include <robot/init.h>

void move_base::MoveBase::planThread()
{
  robot::Timer timer;
  bool wait_for_wake = false;
  boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
  
  while (robot::ok())  // ← Thêm kiểm tra này
  {
    // ... existing code ...
    
    while (wait_for_wake || !runPlanner_)
    {
      if (!robot::ok())  // ← Kiểm tra trước khi wait
        break;
        
      planner_cond_.wait(lock);
      wait_for_wake = false;
    }
    
    // ... rest of code ...
  }
}
```

## Troubleshooting

### Vấn đề: Thread không dừng khi shutdown

**Nguyên nhân**: Không kiểm tra `robot::ok()` trong thread loop

**Giải pháp**:
```cpp
void myThread()
{
  while (robot::ok() && !stop_flag_)  // ← Thêm robot::ok()
  {
    // Work
  }
}
```

### Vấn đề: Callback dài không exit khi shutdown

**Nguyên nhân**: Không kiểm tra `robot::isShuttingDown()`

**Giải pháp**:
```cpp
void longCallback()
{
  for (int i = 0; i < 1000000; i++)
  {
    if (robot::isShuttingDown())  // ← Thêm kiểm tra
      return;
    // Work
  }
}
```

### Vấn đề: Shutdown không hoạt động

**Nguyên nhân**: Chưa gọi `robot::init()`

**Giải pháp**:
```cpp
int main(int argc, char** argv)
{
  robot::init(argc, argv, "my_node");  // ← Phải gọi init() trước
  // ...
}
```

## Tóm tắt

- **`robot::ok()`**: Kiểm tra system đã shutdown hoàn toàn chưa → dùng trong loops
- **`robot::isShuttingDown()`**: Kiểm tra shutdown đã được yêu cầu chưa → dùng trong callback dài
- **`robot::shutdown()`**: Bắt đầu shutdown process
- **`robot::init()`**: Khởi tạo system (phải gọi trước khi dùng các hàm khác)
- **Luôn kiểm tra** trong vòng lặp chính và thread loops
- **Mặc định** tự động xử lý SIGINT (Ctrl-C)

## Tài liệu tham khảo

- [ROS Wiki - Initialization and Shutdown](https://wiki.ros.org/roscpp/Overview/Initialization%20and%20Shutdown)
- `robot_time/ROS_OK_EXPLANATION.md` - Giải thích chi tiết về `ros::ok()`