update
This commit is contained in:
@@ -87,6 +87,7 @@ add_library(${PROJECT_NAME} SHARED
|
||||
src/console.cpp
|
||||
src/node_handle.cpp
|
||||
src/plugin_loader_helper.cpp
|
||||
src/init.cpp
|
||||
)
|
||||
|
||||
# Enable C++17 filesystem feature
|
||||
|
||||
483
src/Libraries/robot_cpp/INIT_USAGE.md
Normal file
483
src/Libraries/robot_cpp/INIT_USAGE.md
Normal file
@@ -0,0 +1,483 @@
|
||||
# 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()`
|
||||
|
||||
@@ -1,14 +1,124 @@
|
||||
# robot_cpp
|
||||
|
||||
Thư viện C++ cung cấp các công cụ tiện ích cho phát triển robot, bao gồm hệ thống logging có màu sắc và quản lý tham số dựa trên YAML (tương tự ROS parameter server).
|
||||
## Tổng quan
|
||||
|
||||
## Mô tả
|
||||
`robot_cpp` là một thư viện C++ cung cấp các công cụ tiện ích cốt lõi cho phát triển ứng dụng robot, tương tự như các utilities trong ROS nhưng hoàn toàn độc lập, không phụ thuộc vào ROS. Thư viện này cung cấp nền tảng cơ bản cho việc xây dựng các hệ thống robot tự động.
|
||||
|
||||
`robot_cpp` là một thư viện hỗ trợ phát triển ứng dụng robot với hai module chính:
|
||||
## Mục đích và phạm vi ứng dụng
|
||||
|
||||
1. **Console Module** (`robot::console`): Hệ thống logging với hỗ trợ màu sắc ANSI, tự động phát hiện terminal, và throttle logging để tránh spam console.
|
||||
Thư viện `robot_cpp` được thiết kế để giải quyết các nhu cầu cơ bản trong phát triển ứng dụng robot:
|
||||
|
||||
2. **NodeHandle Module** (`robot::NodeHandle`): Interface quản lý tham số giống ROS, sử dụng YAML files làm backend, hỗ trợ namespace và cấu trúc tham số phân cấp.
|
||||
- **Logging và Debugging**: Cung cấp hệ thống logging với màu sắc để dễ dàng phân biệt các loại thông điệp
|
||||
- **Quản lý cấu hình**: Hỗ trợ quản lý tham số từ file YAML, tương tự ROS parameter server
|
||||
- **Quản lý vòng đời**: Cung cấp cơ chế quản lý lifecycle của hệ thống robot, xử lý shutdown graceful
|
||||
- **Plugin Loading**: Hỗ trợ tìm kiếm và load các dynamic plugins sử dụng boost::dll
|
||||
|
||||
## Các thành phần chính
|
||||
|
||||
### 1. Console Module - Hệ thống Logging
|
||||
|
||||
Module Console cung cấp hệ thống logging với hỗ trợ màu sắc ANSI để phân biệt các loại thông điệp.
|
||||
|
||||
**Đặc điểm:**
|
||||
- Hỗ trợ nhiều mức độ logging: info, success, warning, error, debug
|
||||
- Tự động phát hiện khả năng hỗ trợ màu của terminal
|
||||
- Throttle logging để giới hạn tần suất log, tránh spam console
|
||||
- Hỗ trợ logging với thông tin file và line number để debug
|
||||
- Tự động reset màu sau khi in để tránh ảnh hưởng đến output tiếp theo
|
||||
- Có thể bật/tắt màu sắc tùy theo môi trường
|
||||
|
||||
**Cơ chế hoạt động:**
|
||||
- Kiểm tra biến môi trường `NO_COLOR` và `TERM` để xác định hỗ trợ màu
|
||||
- Sử dụng static variables để lưu timestamp cho throttle logging
|
||||
- Tự động thêm newline nếu format string không kết thúc bằng newline
|
||||
- Hỗ trợ nhiều màu sắc: red, green, yellow, blue, cyan, magenta, white và các biến thể bright
|
||||
|
||||
**Ứng dụng:**
|
||||
- Debug và monitoring trong quá trình phát triển
|
||||
- Hiển thị trạng thái hệ thống với màu sắc dễ phân biệt
|
||||
- Logging có điều kiện với throttle để tránh spam
|
||||
- Tích hợp vào các hệ thống logging lớn hơn
|
||||
|
||||
### 2. NodeHandle Module - Quản lý Tham số
|
||||
|
||||
Module NodeHandle cung cấp interface quản lý tham số giống ROS parameter server, sử dụng YAML files làm backend.
|
||||
|
||||
**Đặc điểm:**
|
||||
- Hỗ trợ namespace phân cấp giống ROS
|
||||
- Tự động load YAML files từ config directory
|
||||
- Hỗ trợ nhiều kiểu dữ liệu: bool, int, double, float, string, vector, map
|
||||
- Merge nhiều YAML files vào một parameter tree
|
||||
- Thread-safe parameter access
|
||||
- Hỗ trợ nested parameters với cấu trúc phân cấp
|
||||
|
||||
**Cơ chế hoạt động:**
|
||||
- Sử dụng `YAML::Node` để lưu trữ tham số trong memory
|
||||
- Static `root_` node chứa toàn bộ parameter tree, được chia sẻ giữa tất cả NodeHandle instances
|
||||
- Mỗi NodeHandle instance scope vào một namespace cụ thể
|
||||
- Tự động tìm và load YAML files từ config directory khi khởi tạo
|
||||
- Hỗ trợ remapping parameters tương tự ROS
|
||||
|
||||
**Namespace System:**
|
||||
- Root namespace (`""` hoặc `"/"`): Truy cập toàn bộ parameter tree
|
||||
- Private namespace (`"~"`): Map tới config directory của node hiện tại
|
||||
- Nested namespaces: Sử dụng `/` separator (ví dụ: `"robot/base/velocity"`)
|
||||
- Tất cả NodeHandle instances chia sẻ cùng một static parameter tree
|
||||
|
||||
**Ứng dụng:**
|
||||
- Quản lý cấu hình robot từ file YAML
|
||||
- Truyền tham số giữa các modules
|
||||
- Runtime configuration changes
|
||||
- Testing với các bộ tham số khác nhau
|
||||
|
||||
### 3. Init Module - Quản lý Vòng đời Hệ thống
|
||||
|
||||
Module Init quản lý vòng đời của robot system, tương tự `ros::init()` và `ros::ok()` trong ROS.
|
||||
|
||||
**Đặc điểm:**
|
||||
- Quản lý trạng thái system (đang chạy hay đã shutdown)
|
||||
- Xử lý shutdown graceful với signal handling
|
||||
- Tự động xử lý SIGINT (Ctrl-C) để shutdown an toàn
|
||||
- Hỗ trợ custom signal handler
|
||||
- Thread-safe với `std::atomic`
|
||||
|
||||
**Cơ chế hoạt động:**
|
||||
- Sử dụng `std::atomic` để đảm bảo thread-safety
|
||||
- Tự động cài đặt SIGINT handler để xử lý Ctrl-C
|
||||
- Quản lý trạng thái shutdown với 2 flags: `shutdown_requested` và `shutdown_complete`
|
||||
- Hỗ trợ custom signal handler nếu cần xử lý đặc biệt
|
||||
|
||||
**Các trạng thái:**
|
||||
- **Running**: System đang chạy bình thường
|
||||
- **Shutdown Requested**: Shutdown đã được yêu cầu (ngay lập tức khi gọi `shutdown()`)
|
||||
- **Shutdown Complete**: System đã shutdown hoàn toàn
|
||||
|
||||
**Ứng dụng:**
|
||||
- Main loops với kiểm tra `robot::ok()`
|
||||
- Thread loops cần kiểm tra trạng thái system
|
||||
- Long-running callbacks cần early exit khi shutdown
|
||||
- Graceful shutdown với cleanup resources
|
||||
|
||||
### 4. PluginLoaderHelper Module - Tìm kiếm Plugin Libraries
|
||||
|
||||
Module PluginLoaderHelper giúp tìm đường dẫn library file (.so) từ tên symbol (export name) được sử dụng với `BOOST_DLL_ALIAS`.
|
||||
|
||||
**Đặc điểm:**
|
||||
- Map tên symbol (export name) sang đường dẫn library file
|
||||
- Đọc cấu hình từ YAML file hoặc NodeHandle
|
||||
- Tự động tìm build directory tại runtime
|
||||
- Hỗ trợ nhiều cách tìm kiếm: từ config, từ build directory, từ system paths
|
||||
|
||||
**Cơ chế hoạt động:**
|
||||
- Đọc file YAML (`boost_dll_plugins.yaml`) hoặc từ NodeHandle
|
||||
- Map symbol names (ví dụ: "CustomPlanner") sang library paths
|
||||
- Tự động tìm build directory từ environment variables hoặc config
|
||||
- Hỗ trợ fallback mechanisms nếu không tìm thấy trong config
|
||||
|
||||
**Ứng dụng:**
|
||||
- Dynamic loading của global planners, local planners, recovery behaviors
|
||||
- Plugin discovery trong runtime
|
||||
- Tích hợp với boost::dll để load plugins
|
||||
- Quản lý plugin registry
|
||||
|
||||
## Kiến trúc
|
||||
|
||||
@@ -17,358 +127,126 @@ Thư viện C++ cung cấp các công cụ tiện ích cho phát triển robot,
|
||||
```
|
||||
robot_cpp/
|
||||
├── include/robot/
|
||||
│ ├── console.h # Console logging API
|
||||
│ └── node_handle.h # NodeHandle parameter management API
|
||||
│ ├── console.h # Console logging API
|
||||
│ ├── node_handle.h # NodeHandle parameter management API
|
||||
│ ├── init.h # System initialization and shutdown API
|
||||
│ └── plugin_loader_helper.h # Plugin loader helper API
|
||||
├── src/
|
||||
│ ├── console.cpp # Console implementation
|
||||
│ └── node_handle.cpp # NodeHandle implementation
|
||||
│ ├── console.cpp # Console implementation
|
||||
│ ├── node_handle.cpp # NodeHandle implementation
|
||||
│ ├── init.cpp # Init implementation
|
||||
│ └── plugin_loader_helper.cpp # PluginLoaderHelper implementation
|
||||
├── test/
|
||||
│ └── test_node_handle.cpp # Unit tests
|
||||
│ └── test_node_handle.cpp # Unit tests
|
||||
├── INIT_USAGE.md # Hướng dẫn chi tiết về Init module
|
||||
├── PLUGIN_LOADER_README.md # Hướng dẫn về PluginLoaderHelper
|
||||
└── CMakeLists.txt
|
||||
```
|
||||
|
||||
### Module Console
|
||||
### Mối quan hệ giữa các modules
|
||||
|
||||
**Namespace:** `robot::console` và `robot::color`
|
||||
|
||||
**Chức năng:**
|
||||
- Logging với màu sắc ANSI (red, green, yellow, blue, cyan, magenta, white)
|
||||
- Tự động phát hiện hỗ trợ màu của terminal
|
||||
- Throttle logging để giới hạn tần suất log
|
||||
- Hỗ trợ logging với thông tin file và line number
|
||||
- Tự động reset màu sau khi in
|
||||
|
||||
**Cơ chế hoạt động:**
|
||||
- Kiểm tra biến môi trường `NO_COLOR` và `TERM` để xác định hỗ trợ màu
|
||||
- Sử dụng static variables để lưu timestamp cho throttle logging
|
||||
- Tự động thêm newline nếu format string không kết thúc bằng `\n`
|
||||
|
||||
### Module NodeHandle
|
||||
|
||||
**Class:** `robot::NodeHandle`
|
||||
|
||||
**Chức năng:**
|
||||
- Quản lý tham số từ YAML files
|
||||
- Hỗ trợ namespace phân cấp (giống ROS)
|
||||
- Tự động load YAML files từ config directory
|
||||
- Hỗ trợ nhiều kiểu dữ liệu: bool, int, double, float, string, vector, map
|
||||
- Merge nhiều YAML files vào một parameter tree
|
||||
|
||||
**Cơ chế hoạt động:**
|
||||
- Sử dụng `YAML::Node` để lưu trữ tham số
|
||||
- Static `root_` node chứa toàn bộ parameter tree
|
||||
- Mỗi NodeHandle instance scope vào một namespace cụ thể
|
||||
- Tự động tìm và load YAML files từ config directory
|
||||
|
||||
## Cách sử dụng
|
||||
|
||||
### 1. Console Logging
|
||||
|
||||
#### Basic Logging
|
||||
|
||||
```cpp
|
||||
#include "robot/console.h"
|
||||
|
||||
using namespace robot;
|
||||
|
||||
// Log thông tin (màu trắng)
|
||||
log_info("Processing %d items\n", count);
|
||||
|
||||
// Log thành công (màu xanh lá)
|
||||
log_success("Operation completed successfully\n");
|
||||
|
||||
// Log cảnh báo (màu vàng)
|
||||
log_warning("Low battery: %d%%\n", battery_level);
|
||||
|
||||
// Log lỗi (màu đỏ)
|
||||
log_error("Failed to open file: %s\n", filename);
|
||||
|
||||
// Log debug (màu cyan)
|
||||
log_debug("Variable value: %d\n", value);
|
||||
```
|
||||
|
||||
#### Logging với File và Line Number
|
||||
|
||||
```cpp
|
||||
// Tự động thêm thông tin file và line number
|
||||
log_info_at(__FILE__, __LINE__, "Processing started\n");
|
||||
log_error_at(__FILE__, __LINE__, "Error occurred: %s\n", error_msg);
|
||||
```
|
||||
|
||||
#### Throttle Logging
|
||||
|
||||
```cpp
|
||||
// Log tối đa 1 lần mỗi giây
|
||||
log_error_throttle(1.0, "Connection failed: %s\n", error_msg);
|
||||
|
||||
// Log tối đa 10 lần mỗi giây (0.1 giây)
|
||||
log_warning_throttle(0.1, "Low battery: %d%%\n", battery_level);
|
||||
|
||||
// Log info với throttle
|
||||
log_info_throttle(2.0, "Status update: %s\n", status);
|
||||
```
|
||||
|
||||
#### Custom Color Printing
|
||||
|
||||
```cpp
|
||||
// In với màu tùy chỉnh
|
||||
printf_red("Critical error!\n");
|
||||
printf_green("Success!\n");
|
||||
printf_yellow("Warning!\n");
|
||||
|
||||
// Sử dụng color namespace
|
||||
printf_color(color::BRIGHT_RED, "Critical: %s\n", message);
|
||||
printf_color(color::BOLD, "Important message\n");
|
||||
printf_color(color::UNDERLINE, "Underlined text\n");
|
||||
```
|
||||
|
||||
#### Điều khiển màu sắc
|
||||
|
||||
```cpp
|
||||
// Kiểm tra hỗ trợ màu
|
||||
if (is_color_supported()) {
|
||||
log_info("Terminal supports colors\n");
|
||||
}
|
||||
|
||||
// Tắt màu (ví dụ khi redirect output)
|
||||
set_color_enabled(false);
|
||||
log_info("This will be plain text\n");
|
||||
```
|
||||
|
||||
### 2. NodeHandle Parameter Management
|
||||
|
||||
#### Khởi tạo NodeHandle
|
||||
|
||||
```cpp
|
||||
#include "robot/node_handle.h"
|
||||
|
||||
using namespace robot;
|
||||
|
||||
// Root namespace
|
||||
NodeHandle nh;
|
||||
|
||||
// Namespace cụ thể
|
||||
NodeHandle nh_robot("/robot");
|
||||
|
||||
// Private namespace (tự động map tới config directory)
|
||||
NodeHandle nh_private("~");
|
||||
|
||||
// Từ parent NodeHandle
|
||||
NodeHandle nh_base(nh_robot, "/base");
|
||||
```
|
||||
|
||||
#### Đọc tham số
|
||||
|
||||
```cpp
|
||||
// Sử dụng param() với default value (khuyến nghị)
|
||||
int max_iterations = nh.param("max_iterations", 100);
|
||||
double tolerance = nh.param("tolerance", 0.1);
|
||||
std::string frame_id = nh.param("frame_id", std::string("base_link"));
|
||||
bool use_odom = nh.param("use_odometry", true);
|
||||
|
||||
// Sử dụng getParam() với output parameter
|
||||
int value;
|
||||
if (nh.getParam("my_param", value, 0)) {
|
||||
// Parameter tồn tại
|
||||
} else {
|
||||
// Sử dụng default value (0)
|
||||
}
|
||||
|
||||
// Đọc nested parameters
|
||||
double velocity = nh.param("robot/base/max_velocity", 1.0);
|
||||
|
||||
// Đọc vector
|
||||
std::vector<double> limits = nh.param("limits", std::vector<double>{0.0, 1.0, 2.0});
|
||||
|
||||
// Đọc map
|
||||
std::map<std::string, double> weights;
|
||||
nh.getParam("weights", weights);
|
||||
```
|
||||
|
||||
#### Ghi tham số
|
||||
|
||||
```cpp
|
||||
// Set các kiểu dữ liệu khác nhau
|
||||
nh.setParam("max_iterations", 200);
|
||||
nh.setParam("frame_id", std::string("odom"));
|
||||
nh.setParam("enabled", true);
|
||||
|
||||
// Set nested parameter
|
||||
nh.setParam("robot/base/max_velocity", 2.0);
|
||||
|
||||
// Set vector
|
||||
std::vector<int> values = {1, 2, 3, 4};
|
||||
nh.setParam("values", values);
|
||||
|
||||
// Set map
|
||||
std::map<std::string, double> config;
|
||||
config["weight1"] = 0.5;
|
||||
config["weight2"] = 0.3;
|
||||
nh.setParam("config", config);
|
||||
```
|
||||
|
||||
#### Kiểm tra tham số
|
||||
|
||||
```cpp
|
||||
// Kiểm tra tồn tại
|
||||
if (nh.hasParam("my_param")) {
|
||||
// Parameter tồn tại
|
||||
}
|
||||
|
||||
// Tìm kiếm parameter
|
||||
std::string result;
|
||||
if (nh.searchParam("my_param", result)) {
|
||||
// Tìm thấy tại: result
|
||||
}
|
||||
```
|
||||
|
||||
#### Load YAML Files
|
||||
|
||||
```cpp
|
||||
// Tự động load từ config directory (được gọi trong constructor)
|
||||
NodeHandle nh; // Tự động load YAML files
|
||||
|
||||
// Load thủ công từ directory
|
||||
int loaded = nh.loadYamlFilesFromDirectory("/path/to/config");
|
||||
|
||||
// In tất cả parameters (debug)
|
||||
nh.printNodeParams();
|
||||
```
|
||||
|
||||
#### Ví dụ YAML Configuration
|
||||
|
||||
```yaml
|
||||
# config/robot_params.yaml
|
||||
robot:
|
||||
base:
|
||||
max_velocity: 2.0
|
||||
max_acceleration: 1.0
|
||||
frame_id: "base_link"
|
||||
|
||||
sensors:
|
||||
laser:
|
||||
enabled: true
|
||||
range: 10.0
|
||||
frequency: 10.0
|
||||
|
||||
camera:
|
||||
enabled: false
|
||||
resolution: [1920, 1080]
|
||||
|
||||
limits:
|
||||
- 0.0
|
||||
- 1.0
|
||||
- 2.0
|
||||
```
|
||||
|
||||
Sử dụng trong code:
|
||||
|
||||
```cpp
|
||||
NodeHandle nh;
|
||||
|
||||
// Đọc từ YAML
|
||||
double max_vel = nh.param("robot/base/max_velocity", 1.0);
|
||||
bool laser_enabled = nh.param("robot/sensors/laser/enabled", false);
|
||||
std::vector<double> limits = nh.param("robot/limits", std::vector<double>());
|
||||
```
|
||||
|
||||
### 3. Ví dụ tích hợp
|
||||
|
||||
```cpp
|
||||
#include "robot/console.h"
|
||||
#include "robot/node_handle.h"
|
||||
|
||||
int main() {
|
||||
using namespace robot;
|
||||
|
||||
// Khởi tạo NodeHandle và load config
|
||||
NodeHandle nh;
|
||||
|
||||
// Đọc tham số
|
||||
int max_iter = nh.param("max_iterations", 100);
|
||||
double tolerance = nh.param("tolerance", 0.1);
|
||||
|
||||
log_info("Starting with max_iterations=%d, tolerance=%.2f\n",
|
||||
max_iter, tolerance);
|
||||
|
||||
// Vòng lặp xử lý
|
||||
for (int i = 0; i < max_iter; ++i) {
|
||||
// Throttle log để tránh spam
|
||||
log_info_throttle(1.0, "Iteration %d/%d\n", i+1, max_iter);
|
||||
|
||||
// Xử lý...
|
||||
|
||||
if (some_error_condition) {
|
||||
log_error_throttle(0.5, "Error at iteration %d\n", i);
|
||||
}
|
||||
}
|
||||
|
||||
log_success("Processing completed\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
```
|
||||
- **Console** là module độc lập, không phụ thuộc vào modules khác
|
||||
- **NodeHandle** sử dụng yaml-cpp để parse YAML files
|
||||
- **Init** quản lý lifecycle chung của system, được sử dụng bởi tất cả modules khác
|
||||
- **PluginLoaderHelper** sử dụng NodeHandle để đọc cấu hình plugins
|
||||
|
||||
## Dependencies
|
||||
|
||||
- **yaml-cpp**: Thư viện parse YAML files
|
||||
- **robot_xmlrpcpp**: Hỗ trợ XmlRpcValue conversion (optional)
|
||||
- **yaml-cpp**: Thư viện parse YAML files (cho NodeHandle và PluginLoaderHelper)
|
||||
- **robot_xmlrpcpp**: Hỗ trợ XmlRpcValue conversion (optional, cho NodeHandle)
|
||||
- **robot_time**: Thư viện quản lý thời gian (cho Init module với Rate/Duration)
|
||||
- **C++17**: Yêu cầu C++ standard 17 trở lên
|
||||
- **Boost.DLL**: Được sử dụng bởi PluginLoaderHelper (thông qua boost::dll)
|
||||
|
||||
## Build
|
||||
## Build và Installation
|
||||
|
||||
Thư viện được build cùng với project chính thông qua CMake:
|
||||
Thư viện hỗ trợ cả Catkin và Standalone CMake:
|
||||
|
||||
```bash
|
||||
cd build
|
||||
cmake ..
|
||||
make
|
||||
```
|
||||
- **Với Catkin**: Tích hợp vào catkin workspace và build như các ROS packages khác
|
||||
- **Với Standalone CMake**: Có thể build độc lập, không cần ROS
|
||||
|
||||
## API Reference
|
||||
## Use Cases
|
||||
|
||||
### Console Functions
|
||||
### Configuration Management
|
||||
Sử dụng NodeHandle để quản lý tất cả cấu hình robot từ file YAML, cho phép thay đổi cấu hình mà không cần recompile.
|
||||
|
||||
| Function | Mô tả |
|
||||
|----------|-------|
|
||||
| `log_info(format, ...)` | Log thông tin (màu trắng) |
|
||||
| `log_success(format, ...)` | Log thành công (màu xanh lá) |
|
||||
| `log_warning(format, ...)` | Log cảnh báo (màu vàng) |
|
||||
| `log_error(format, ...)` | Log lỗi (màu đỏ) |
|
||||
| `log_debug(format, ...)` | Log debug (màu cyan) |
|
||||
| `log_*_at(file, line, format, ...)` | Log với file và line number |
|
||||
| `log_*_throttle(period, format, ...)` | Log với throttle (giới hạn tần suất) |
|
||||
| `printf_*()` | In với màu cụ thể |
|
||||
| `is_color_enabled()` | Kiểm tra màu có được bật |
|
||||
| `set_color_enabled(bool)` | Bật/tắt màu |
|
||||
### System Logging
|
||||
Sử dụng Console module để log thông tin, cảnh báo, và lỗi với màu sắc, giúp dễ dàng debug và monitor hệ thống.
|
||||
|
||||
### NodeHandle Methods
|
||||
### Lifecycle Management
|
||||
Sử dụng Init module để quản lý vòng đời của ứng dụng, đảm bảo shutdown graceful khi nhận Ctrl-C hoặc signal khác.
|
||||
|
||||
| Method | Mô tả |
|
||||
|--------|-------|
|
||||
| `param<T>(key, default)` | Đọc tham số với default value |
|
||||
| `getParam(key, value, default)` | Đọc tham số với output parameter |
|
||||
| `setParam(key, value)` | Ghi tham số |
|
||||
| `hasParam(key)` | Kiểm tra tham số tồn tại |
|
||||
| `searchParam(key, result)` | Tìm kiếm tham số |
|
||||
| `loadYamlFilesFromDirectory(path)` | Load YAML files từ directory |
|
||||
| `printNodeParams()` | In tất cả tham số (debug) |
|
||||
### Plugin Discovery
|
||||
Sử dụng PluginLoaderHelper để tìm và load các plugins động tại runtime, cho phép mở rộng hệ thống mà không cần recompile.
|
||||
|
||||
## Lưu ý
|
||||
### Multi-threaded Applications
|
||||
Tất cả các modules đều thread-safe, cho phép sử dụng an toàn trong các ứng dụng đa luồng.
|
||||
|
||||
1. **Throttle Logging**: Mỗi hàm throttle sử dụng static variable riêng để lưu timestamp. Các lời gọi từ cùng một vị trí code sẽ chia sẻ cùng một throttle counter.
|
||||
## Best Practices
|
||||
|
||||
2. **Color Support**: Màu sắc tự động bị tắt nếu:
|
||||
- Biến môi trường `NO_COLOR` được set
|
||||
- Terminal không hỗ trợ ANSI colors
|
||||
- Output được redirect vào file
|
||||
1. **Sử dụng NodeHandle cho tất cả cấu hình**: Tránh hardcode parameters, sử dụng YAML files
|
||||
|
||||
3. **NodeHandle Namespace**:
|
||||
- Namespace `"~"` hoặc `""` map tới root namespace `/`
|
||||
- Nested parameters sử dụng `/` separator (ví dụ: `"robot/base/velocity"`)
|
||||
- Tất cả NodeHandle instances chia sẻ cùng một static parameter tree
|
||||
2. **Sử dụng Console logging thay vì printf/cout**: Có màu sắc và throttle logging tự động
|
||||
|
||||
4. **YAML Loading**: NodeHandle tự động tìm và load YAML files từ config directory khi khởi tạo. Files được merge theo thứ tự filesystem.
|
||||
3. **Luôn kiểm tra robot::ok() trong loops**: Đảm bảo có thể shutdown gracefully
|
||||
|
||||
4. **Sử dụng throttle logging cho frequent messages**: Tránh spam console với các messages lặp lại
|
||||
|
||||
5. **Namespace organization**: Sử dụng namespace phân cấp để tổ chức parameters một cách logic
|
||||
|
||||
6. **Plugin configuration**: Sử dụng PluginLoaderHelper thay vì hardcode library paths
|
||||
|
||||
## Tính năng nổi bật
|
||||
|
||||
### 1. Color Support với Auto-detection
|
||||
Console module tự động phát hiện khả năng hỗ trợ màu của terminal và tự động tắt màu nếu:
|
||||
- Biến môi trường `NO_COLOR` được set
|
||||
- Terminal không hỗ trợ ANSI colors
|
||||
- Output được redirect vào file
|
||||
|
||||
### 2. Throttle Logging
|
||||
Mỗi hàm throttle sử dụng static variable riêng để lưu timestamp, cho phép:
|
||||
- Giới hạn tần suất log của các messages lặp lại
|
||||
- Tránh spam console với các messages thường xuyên
|
||||
- Tự động reset sau một khoảng thời gian
|
||||
|
||||
### 3. YAML Auto-loading
|
||||
NodeHandle tự động tìm và load YAML files từ config directory khi khởi tạo:
|
||||
- Tìm tất cả `.yaml` và `.yml` files trong config directory
|
||||
- Merge tất cả files vào một parameter tree
|
||||
- Hỗ trợ nested structures và arrays
|
||||
|
||||
### 4. Thread-safe Operations
|
||||
Tất cả các operations đều thread-safe:
|
||||
- NodeHandle sử dụng static parameter tree với thread-safe access
|
||||
- Init module sử dụng `std::atomic` cho flags
|
||||
- Console logging thread-safe với static variables
|
||||
|
||||
### 5. Graceful Shutdown
|
||||
Init module cung cấp cơ chế shutdown graceful:
|
||||
- Tự động xử lý SIGINT (Ctrl-C)
|
||||
- Hỗ trợ custom signal handlers
|
||||
- Phân biệt giữa "shutdown requested" và "shutdown complete"
|
||||
- Cho phép early exit trong long-running callbacks
|
||||
|
||||
## Lưu ý quan trọng
|
||||
|
||||
- **NodeHandle static tree**: Tất cả NodeHandle instances chia sẻ cùng một static parameter tree, thay đổi từ một instance sẽ ảnh hưởng đến tất cả instances khác
|
||||
|
||||
- **YAML loading order**: YAML files được load và merge theo thứ tự filesystem, file sau sẽ override parameters của file trước nếu có conflict
|
||||
|
||||
- **Throttle logging scope**: Mỗi hàm throttle sử dụng static variable riêng, các lời gọi từ cùng một vị trí code sẽ chia sẻ cùng một throttle counter
|
||||
|
||||
- **Plugin path resolution**: PluginLoaderHelper tìm library paths theo thứ tự: config file → build directory → system paths
|
||||
|
||||
- **Init timing**: Cần gọi `robot::init()` trước khi sử dụng các modules khác, đặc biệt là `robot::Time::now()`
|
||||
|
||||
## Tài liệu tham khảo
|
||||
|
||||
- `INIT_USAGE.md` - Hướng dẫn chi tiết về Init module với các use cases và best practices
|
||||
- `PLUGIN_LOADER_README.md` - Hướng dẫn về PluginLoaderHelper và cách cấu hình plugins
|
||||
- `README.md` (file này) - Tổng quan về toàn bộ thư viện
|
||||
|
||||
## License
|
||||
|
||||
Xem file LICENSE trong thư mục gốc.
|
||||
BSD License - Xem file LICENSE trong thư mục gốc.
|
||||
|
||||
134
src/Libraries/robot_cpp/include/robot/init.h
Normal file
134
src/Libraries/robot_cpp/include/robot/init.h
Normal file
@@ -0,0 +1,134 @@
|
||||
#ifndef ROBOT_INIT_H_INCLUDED
|
||||
#define ROBOT_INIT_H_INCLUDED
|
||||
|
||||
#include <atomic>
|
||||
#include <csignal>
|
||||
#include <string>
|
||||
|
||||
namespace robot
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Check if the robot system is still running
|
||||
*
|
||||
* Similar to ros::ok(), this function returns false when the system
|
||||
* has finished shutting down. Use this in main loops and thread loops
|
||||
* to check if the system should continue running.
|
||||
*
|
||||
* @return true if system is running, false if shutdown is complete
|
||||
*
|
||||
* @note This returns false only after shutdown is complete, not when
|
||||
* shutdown is requested. Use isShuttingDown() to check if shutdown
|
||||
* has been requested.
|
||||
*
|
||||
* Example:
|
||||
* @code
|
||||
* while (robot::ok())
|
||||
* {
|
||||
* // Do work
|
||||
* }
|
||||
* @endcode
|
||||
*/
|
||||
bool ok();
|
||||
|
||||
/**
|
||||
* @brief Check if shutdown has been requested
|
||||
*
|
||||
* Similar to ros::isShuttingDown(), this function returns true as soon
|
||||
* as shutdown() is called, not when shutdown is complete. Use this in
|
||||
* long-running callbacks to check if they should exit early.
|
||||
*
|
||||
* @return true if shutdown has been requested, false otherwise
|
||||
*
|
||||
* @note This returns true immediately when shutdown() is called, while
|
||||
* ok() returns false only after shutdown is complete.
|
||||
*
|
||||
* Example:
|
||||
* @code
|
||||
* void longCallback()
|
||||
* {
|
||||
* for (int i = 0; i < 1000000; i++)
|
||||
* {
|
||||
* if (robot::isShuttingDown())
|
||||
* return; // Exit early
|
||||
* // Do work...
|
||||
* }
|
||||
* }
|
||||
* @endcode
|
||||
*/
|
||||
bool isShuttingDown();
|
||||
|
||||
/**
|
||||
* @brief Shutdown the robot system
|
||||
*
|
||||
* Similar to ros::shutdown(), this function initiates the shutdown process.
|
||||
* After calling this:
|
||||
* - isShuttingDown() will return true immediately
|
||||
* - ok() will return false after shutdown is complete
|
||||
*
|
||||
* @note This function is thread-safe and can be called multiple times.
|
||||
* Subsequent calls have no effect.
|
||||
*
|
||||
* Example:
|
||||
* @code
|
||||
* // Custom signal handler
|
||||
* void mySigintHandler(int sig)
|
||||
* {
|
||||
* // Do custom cleanup
|
||||
* robot::shutdown();
|
||||
* }
|
||||
* @endcode
|
||||
*/
|
||||
void shutdown();
|
||||
|
||||
/**
|
||||
* @brief Initialize the robot system
|
||||
*
|
||||
* Similar to ros::init(), this function initializes the robot system.
|
||||
* It sets up signal handlers and initializes internal state.
|
||||
*
|
||||
* @param argc Number of command line arguments (can be modified)
|
||||
* @param argv Command line arguments (can be modified)
|
||||
* @param node_name Name of the node (optional, for logging purposes)
|
||||
* @param install_sigint_handler If true, install SIGINT handler (default: true)
|
||||
*
|
||||
* @note If install_sigint_handler is true, Ctrl-C will automatically
|
||||
* call shutdown(). Set to false if you want custom signal handling.
|
||||
*
|
||||
* Example:
|
||||
* @code
|
||||
* int main(int argc, char** argv)
|
||||
* {
|
||||
* robot::init(argc, argv, "my_node");
|
||||
* // ... use robot system ...
|
||||
* return 0;
|
||||
* }
|
||||
* @endcode
|
||||
*/
|
||||
void init(int& argc, char** argv, const std::string& node_name = "", bool install_sigint_handler = true);
|
||||
|
||||
/**
|
||||
* @brief Initialize the robot system without command line arguments
|
||||
*
|
||||
* Simplified version of init() that doesn't require argc/argv.
|
||||
*
|
||||
* @param node_name Name of the node (optional, for logging purposes)
|
||||
* @param install_sigint_handler If true, install SIGINT handler (default: true)
|
||||
*/
|
||||
void init(const std::string& node_name = "", bool install_sigint_handler = true);
|
||||
|
||||
namespace detail
|
||||
{
|
||||
/**
|
||||
* @brief Internal signal handler for SIGINT
|
||||
*
|
||||
* This is called automatically when SIGINT is received (Ctrl-C).
|
||||
* It calls shutdown() to gracefully shut down the system.
|
||||
*/
|
||||
void sigintHandler(int sig);
|
||||
}
|
||||
|
||||
} // namespace robot
|
||||
|
||||
#endif // ROBOT_INIT_H_INCLUDED
|
||||
|
||||
@@ -2,12 +2,15 @@
|
||||
<name>robot_cpp</name>
|
||||
<version>0.7.10</version>
|
||||
<description>
|
||||
robot_cpp is the second generation of the transform library, which lets
|
||||
the user keep track of multiple coordinate frames over time. robot_cpp
|
||||
maintains the relationship between coordinate frames in a tree
|
||||
structure buffered in time, and lets the user transform points,
|
||||
vectors, etc between any two coordinate frames at any desired
|
||||
point in time.
|
||||
robot_cpp provides core utilities for robot application development, including:
|
||||
- Console: Colored logging system with ANSI color support and throttle logging
|
||||
- NodeHandle: ROS-like parameter management using YAML files as backend
|
||||
- Init: System lifecycle management (init, ok, shutdown) with graceful shutdown handling
|
||||
- PluginLoaderHelper: Helper to find library paths from symbol names for boost::dll plugins
|
||||
|
||||
The library is designed to be independent of ROS and provides a foundation for
|
||||
building robot applications with configuration management, logging, and plugin loading.
|
||||
All operations are thread-safe and support both Catkin and Standalone CMake builds.
|
||||
</description>
|
||||
<author>Tully Foote</author>
|
||||
<author>Eitan Marder-Eppstein</author>
|
||||
|
||||
139
src/Libraries/robot_cpp/src/init.cpp
Normal file
139
src/Libraries/robot_cpp/src/init.cpp
Normal file
@@ -0,0 +1,139 @@
|
||||
#include <robot/init.h>
|
||||
#include <robot/console.h>
|
||||
#include <csignal>
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
|
||||
namespace robot
|
||||
{
|
||||
|
||||
namespace detail
|
||||
{
|
||||
// Internal state for shutdown management
|
||||
std::atomic<bool> g_shutdown_requested(false);
|
||||
std::atomic<bool> g_shutdown_complete(false);
|
||||
std::atomic<bool> g_initialized(false);
|
||||
std::mutex g_shutdown_mutex;
|
||||
bool g_sigint_handler_installed = false;
|
||||
|
||||
// Original SIGINT handler (if any)
|
||||
void (*g_original_sigint_handler)(int) = nullptr;
|
||||
}
|
||||
|
||||
void detail::sigintHandler(int sig)
|
||||
{
|
||||
(void)sig; // Suppress unused parameter warning
|
||||
|
||||
// Call shutdown
|
||||
robot::shutdown();
|
||||
|
||||
// If there was an original handler, call it too
|
||||
if (detail::g_original_sigint_handler &&
|
||||
detail::g_original_sigint_handler != SIG_DFL &&
|
||||
detail::g_original_sigint_handler != SIG_IGN)
|
||||
{
|
||||
detail::g_original_sigint_handler(sig);
|
||||
}
|
||||
}
|
||||
|
||||
bool ok()
|
||||
{
|
||||
// Return false if shutdown is complete
|
||||
return !detail::g_shutdown_complete.load(std::memory_order_acquire);
|
||||
}
|
||||
|
||||
bool isShuttingDown()
|
||||
{
|
||||
// Return true if shutdown has been requested
|
||||
return detail::g_shutdown_requested.load(std::memory_order_acquire);
|
||||
}
|
||||
|
||||
void shutdown()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(detail::g_shutdown_mutex);
|
||||
|
||||
// If already shutting down, do nothing
|
||||
if (detail::g_shutdown_requested.load(std::memory_order_acquire))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Mark shutdown as requested
|
||||
detail::g_shutdown_requested.store(true, std::memory_order_release);
|
||||
|
||||
// Log shutdown
|
||||
robot::log_info("Shutting down robot system...");
|
||||
|
||||
// TODO: Add cleanup of resources here if needed
|
||||
// For example:
|
||||
// - Close all subscriptions/publications
|
||||
// - Stop all timers
|
||||
// - Cleanup threads
|
||||
|
||||
// Mark shutdown as complete
|
||||
// In a more complex system, you might want to wait for resources to cleanup
|
||||
detail::g_shutdown_complete.store(true, std::memory_order_release);
|
||||
|
||||
robot::log_info("Robot system shutdown complete");
|
||||
}
|
||||
|
||||
void init(int& argc, char** argv, const std::string& node_name, bool install_sigint_handler)
|
||||
{
|
||||
(void)argc; // Suppress unused parameter warning
|
||||
(void)argv; // Suppress unused parameter warning
|
||||
|
||||
std::lock_guard<std::mutex> lock(detail::g_shutdown_mutex);
|
||||
|
||||
// If already initialized, do nothing
|
||||
if (detail::g_initialized.load(std::memory_order_acquire))
|
||||
{
|
||||
robot::log_warning("robot::init() called multiple times, ignoring");
|
||||
return;
|
||||
}
|
||||
|
||||
// Reset shutdown state
|
||||
detail::g_shutdown_requested.store(false, std::memory_order_release);
|
||||
detail::g_shutdown_complete.store(false, std::memory_order_release);
|
||||
|
||||
// Install SIGINT handler if requested
|
||||
if (install_sigint_handler)
|
||||
{
|
||||
// Save original handler
|
||||
detail::g_original_sigint_handler = std::signal(SIGINT, detail::sigintHandler);
|
||||
detail::g_sigint_handler_installed = true;
|
||||
|
||||
if (!node_name.empty())
|
||||
{
|
||||
robot::log_info("Initialized robot system for node: %s", node_name.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
robot::log_info("Initialized robot system");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!node_name.empty())
|
||||
{
|
||||
robot::log_info("Initialized robot system for node: %s (no SIGINT handler)", node_name.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
robot::log_info("Initialized robot system (no SIGINT handler)");
|
||||
}
|
||||
}
|
||||
|
||||
// Mark as initialized
|
||||
detail::g_initialized.store(true, std::memory_order_release);
|
||||
}
|
||||
|
||||
void init(const std::string& node_name, bool install_sigint_handler)
|
||||
{
|
||||
// Dummy argc/argv for the other init() function
|
||||
int dummy_argc = 0;
|
||||
char** dummy_argv = nullptr;
|
||||
init(dummy_argc, dummy_argv, node_name, install_sigint_handler);
|
||||
}
|
||||
|
||||
} // namespace robot
|
||||
|
||||
Reference in New Issue
Block a user