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

#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

#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

#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

#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

#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

#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

#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:

#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:

#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

   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

   while (robot::ok())
   {
     // Do work
   }
   

3. Kiểm tra trong thread loops

   while (robot::ok() && !stop_flag_)
   {
     // Thread work
   }
   

4. Dùng robot::isShuttingDown() trong callback dài

   if (robot::isShuttingDown()) return;
   

5. Kết hợp với các điều kiện khác

   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():

#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:

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:

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:

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
• robot_time/ROS_OK_EXPLANATION.md - Giải thích chi tiết về ros::ok()