generated from wessel/boilerplate
fix(hw_interface): Increase buffersize by 2
This commit is contained in:
452
doc/architecture/nodes/HardwareInterface.md
Normal file
452
doc/architecture/nodes/HardwareInterface.md
Normal file
@@ -0,0 +1,452 @@
|
||||
# HardwareInterface (`assignments::two::g2_2025_lifecycle_node`)
|
||||
|
||||
## Overview
|
||||
The `HardwareInterface` is a ROS2 node responsible for managing low-level hardware communication with IMU sensors. It abstracts the complexity of dual communication backends (serial and MQTT), provides JSON parsing of sensor data, and publishes standardized `sensor_msgs::msg::Imu` messages to the ROS2 ecosystem. The node handles continuous data acquisition, buffering of fragmented reads, and robust error recovery.
|
||||
|
||||
#### Implementation Details
|
||||
|
||||
**Public Methods**
|
||||
|
||||
- **`start_read()`**: Spawns a background thread that continuously reads JSON payloads from the serial device
|
||||
- **`stop_read()`**: Signals the reader thread to exit and joins it for clean shutdown
|
||||
- **`write(const std::string& data)`**: Writes data to the serial device
|
||||
- **`open_device(const std::string& device_path, int baud_rate)`**: Opens and configures a serial port
|
||||
- **`is_device_open()`**: Checks if the serial device is currently open
|
||||
- **`close_device()`**: Closes the serial port and releases resources
|
||||
- **`mqtt_configure()`**: Initializes MQTT client and broker connection setup
|
||||
- **`mqtt_reader()`**: Attaches MQTT callbacks to begin receiving messages
|
||||
- **`mqtt_connect()`**: Establishes connection to the MQTT broker and subscribes to topics
|
||||
- **`close_mqtt_conn()`**: Disconnects from broker and cleans up MQTT resources
|
||||
- **`parse_data(const std::string& data)`**: Parses JSON payload into `sensor_msgs::msg::Imu` and publishes
|
||||
- **`publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg)`**: Publishes an IMU message to the ROS topic
|
||||
|
||||
**Constructor**
|
||||
|
||||
```cpp
|
||||
HardwareInterface()
|
||||
```
|
||||
|
||||
- Initializes ROS2 node with name `hardware_interface`
|
||||
- Creates a ROS2 publisher for `sensor_msgs::msg::Imu` on topic `imu_data` with queue size 10
|
||||
- Logs initialization status
|
||||
|
||||
**Member Variables**
|
||||
|
||||
- **`serialib serial`**: Encapsulates serial port communication
|
||||
- **`std::shared_ptr<mqtt::async_client> mqtt_client`**: Persistent MQTT async client for broker communication
|
||||
- **`std::shared_ptr<mqtt::callback> mqtt_cb`**: MQTT callback handler for message arrival events
|
||||
- **`std::thread read_thread_`**: Background reader thread for continuous serial data acquisition
|
||||
- **`std::atomic_bool reading_`**: Thread-safe flag to signal the reader thread to stop
|
||||
- **`std::string partial_buffer_`**: Accumulates fragmented serial reads until complete messages are available
|
||||
- **`rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher`**: ROS2 publisher for IMU data
|
||||
|
||||
**MQTT Constants**
|
||||
|
||||
- **`SERVER_ADDRESS`**: `"tcp://localhost:1883"` — Default MQTT broker address
|
||||
- **`CLIENT_ID`**: `"cpp_mqtt_client"` — MQTT client identifier
|
||||
- **`TOPIC`**: `"esp32/imu"` — Default subscription topic for IMU data
|
||||
|
||||
---
|
||||
|
||||
## Core Functions
|
||||
|
||||
### `void start_read()`
|
||||
|
||||
Initiates continuous serial data acquisition in a background thread.
|
||||
|
||||
**Behavior:**
|
||||
- Checks if a reader thread is already running; returns if so
|
||||
- Sets the `reading_` atomic flag to true
|
||||
- Spawns a thread that:
|
||||
1. Allocates a 116-byte buffer
|
||||
2. Enters a loop that runs while `reading_` is true
|
||||
3. Calls `serial.readString()` with a 1-second timeout
|
||||
4. Accumulates received bytes into `partial_buffer_`
|
||||
5. Splits on newline (`\n`) to extract complete lines
|
||||
6. Trims whitespace and strips leading garbage up to the first `{`
|
||||
7. Validates that a closing `}` exists; if not, waits for more data
|
||||
8. Calls `parse_data()` on each complete JSON line
|
||||
- Returns immediately while the thread continues running
|
||||
|
||||
**Error Handling:**
|
||||
- Invalid JSON lines are logged as errors in `parse_data()` but do not crash the thread
|
||||
|
||||
---
|
||||
|
||||
### `void stop_read()`
|
||||
|
||||
Cleanly terminates the background reader thread.
|
||||
|
||||
**Behavior:**
|
||||
- Returns immediately if not currently reading
|
||||
- Sets `reading_` atomic flag to false to signal the thread
|
||||
- Joins the thread to wait for its completion
|
||||
- Ensures all resources are released before returning
|
||||
|
||||
**Thread Safety:**
|
||||
- Uses atomic flag for lock-free signaling
|
||||
- Blocks until thread joins, guaranteeing clean shutdown
|
||||
|
||||
---
|
||||
|
||||
### `void parse_data(const std::string& data)`
|
||||
|
||||
Deserializes a JSON string into a ROS2 IMU message and publishes it.
|
||||
|
||||
**Behavior:**
|
||||
- Attempts to parse the input string as JSON using `nlohmann::json`
|
||||
- Creates a new `sensor_msgs::msg::Imu` message and populates:
|
||||
- **Header**: Sets `stamp` to current time via `this->now()` and `frame_id` to `"imu_link"`
|
||||
- **Linear Acceleration**: Extracts from JSON `"accel"` object fields `"x"`, `"y"`, `"z"` (defaults to 0.0 if missing)
|
||||
- **Angular Velocity**: Extracts from JSON `"gyro"` object fields `"x"`, `"y"`, `"z"` (defaults to 0.0 if missing)
|
||||
- Logs the parsed IMU values at INFO level
|
||||
- Calls `publish_imu_data()` to send the message to the ROS topic
|
||||
|
||||
**Expected JSON Format:**
|
||||
```json
|
||||
{
|
||||
"accel": {"x": 0.037, "y": -1.164, "z": 9.775},
|
||||
"gyro": {"x": -0.024, "y": -0.014, "z": -0.001},
|
||||
"Temp": 41.01
|
||||
}
|
||||
```
|
||||
|
||||
**Error Handling:**
|
||||
- Catches `nlohmann::json::exception` and logs parsing errors without crashing
|
||||
- Handles missing fields gracefully using `.value()` with default 0.0
|
||||
|
||||
---
|
||||
|
||||
### `void publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg)`
|
||||
|
||||
Publishes an IMU message to the ROS2 topic.
|
||||
|
||||
**Behavior:**
|
||||
- Dereferences the shared pointer and publishes to `imu_publisher`
|
||||
- Operation is thread-safe (rclcpp publishers support multi-threaded access)
|
||||
|
||||
---
|
||||
|
||||
### `void mqtt_configure()`
|
||||
|
||||
Sets up the MQTT infrastructure for broker communication.
|
||||
|
||||
**Behavior:**
|
||||
- Creates a persistent `mqtt::async_client` pointing to `SERVER_ADDRESS` if not already created
|
||||
- Creates a persistent MQTT callback handler if not already created
|
||||
- Calls `mqtt_connect()` to establish the connection
|
||||
|
||||
**Rationale for Persistence:**
|
||||
- Client and callback objects must outlive this function to maintain the connection
|
||||
- Using `shared_ptr` ensures proper lifetime management
|
||||
|
||||
---
|
||||
|
||||
### `void mqtt_reader()`
|
||||
|
||||
Attaches callbacks to the MQTT client to begin receiving messages.
|
||||
|
||||
**Behavior:**
|
||||
- Sets the callback handler on the async client via `mqtt_client->set_callback(*mqtt_cb)`
|
||||
- Logs that the listener has started
|
||||
- Returns; the async client handles message reception in background threads
|
||||
|
||||
---
|
||||
|
||||
### `void mqtt_connect()`
|
||||
|
||||
Establishes connection to the MQTT broker and subscribes to the sensor topic.
|
||||
|
||||
**Behavior:**
|
||||
- Creates `mqtt::connect_options` with:
|
||||
- Keep-alive interval: 20 seconds
|
||||
- Clean session: true (no prior session state restored)
|
||||
- Calls `mqtt_client->connect()` and waits for completion
|
||||
- Subscribes to `TOPIC` (default: `"esp32/imu"`) with QoS level 1
|
||||
- Logs successful connection and subscription
|
||||
|
||||
**Error Handling:**
|
||||
- Catches `mqtt::exception` and logs errors; does not throw or crash
|
||||
|
||||
---
|
||||
|
||||
### `void close_mqtt_conn()`
|
||||
|
||||
Cleanly disconnects from the MQTT broker and cleans up resources.
|
||||
|
||||
**Behavior:**
|
||||
- Checks if the client is connected before attempting disconnect
|
||||
- Calls `mqtt_client->disconnect()` and waits for completion
|
||||
- Resets `mqtt_client` and `mqtt_cb` shared pointers to allow object destruction
|
||||
- Logs disconnection and cleanup status
|
||||
|
||||
**Error Handling:**
|
||||
- Catches `mqtt::exception` and logs errors
|
||||
- Continues cleanup even if errors occur
|
||||
|
||||
---
|
||||
|
||||
### `bool open_device(const std::string& device_path, int baud_rate)`
|
||||
|
||||
Opens and configures a serial port device.
|
||||
|
||||
**Parameters:**
|
||||
- `device_path`: Path to the serial device (e.g., `"/dev/ttyUSB0"`)
|
||||
- `baud_rate`: Communication speed in bits per second (e.g., `115200`)
|
||||
|
||||
**Returns:**
|
||||
- `true` if device opened successfully
|
||||
- `false` if an error occurs
|
||||
|
||||
**Behavior:**
|
||||
- Calls `serial.openDevice()` with the provided path and baud rate
|
||||
- Checks if the returned value is 1 (success)
|
||||
- Logs success or error status
|
||||
|
||||
---
|
||||
|
||||
### `bool is_device_open()`
|
||||
|
||||
Queries the current state of the serial device.
|
||||
|
||||
**Returns:**
|
||||
- `true` if the device is open
|
||||
- `false` otherwise
|
||||
|
||||
---
|
||||
|
||||
### `void close_device()`
|
||||
|
||||
Closes the serial port and releases resources.
|
||||
|
||||
**Behavior:**
|
||||
- Calls `serial.closeDevice()`
|
||||
- Ensures the device is no longer accessible for reads/writes
|
||||
|
||||
---
|
||||
|
||||
### `void write(const std::string& data)`
|
||||
|
||||
Writes data to the serial device.
|
||||
|
||||
**Behavior:**
|
||||
- Logs the write operation
|
||||
- Calls `serial.writeString()` with the data
|
||||
|
||||
---
|
||||
|
||||
## MQTT Callback Handler
|
||||
|
||||
### `class callback : public virtual mqtt::callback`
|
||||
|
||||
A nested class that implements the Paho MQTT callback interface.
|
||||
|
||||
**Method: `message_arrived(mqtt::const_message_ptr msg)`**
|
||||
|
||||
- Invoked when a message arrives on a subscribed topic
|
||||
- Extracts the payload string via `msg->get_payload_str()`
|
||||
- Calls `parse_data()` to deserialize and publish the IMU message
|
||||
---
|
||||
|
||||
## Data Flow Architecture
|
||||
|
||||
### Serial Data Path
|
||||
|
||||
```
|
||||
Physical IMU Device
|
||||
↓
|
||||
Serial Port (e.g., /dev/ttyUSB0 @ 115200 baud)
|
||||
↓
|
||||
start_read() Background Thread
|
||||
↓
|
||||
serial.readString(buffer, 1000ms timeout)
|
||||
↓
|
||||
Accumulate into partial_buffer_
|
||||
↓
|
||||
Split on '\n' and Extract Complete Lines
|
||||
↓
|
||||
Sanitize (trim, strip garbage before '{')
|
||||
↓
|
||||
Validate JSON Structure (must have '{' and '}')
|
||||
↓
|
||||
parse_data(json_line)
|
||||
↓
|
||||
JSON Parse → sensor_msgs::msg::Imu
|
||||
↓
|
||||
publish_imu_data() → ROS Topic `imu_data`
|
||||
```
|
||||
|
||||
### MQTT Data Path
|
||||
|
||||
```
|
||||
MQTT Broker (tcp://localhost:1883)
|
||||
↓
|
||||
MQTT Async Client (mqtt_client)
|
||||
↓
|
||||
Topic Subscription (esp32/imu)
|
||||
↓
|
||||
MQTT Callback (message_arrived)
|
||||
↓
|
||||
parse_data(payload_string)
|
||||
↓
|
||||
JSON Parse → sensor_msgs::msg::Imu
|
||||
↓
|
||||
publish_imu_data() → ROS Topic `imu_data`
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Buffer Management & Message Reconstruction
|
||||
|
||||
The `partial_buffer_` member implements a robust strategy for handling fragmented serial reads:
|
||||
|
||||
1. **Accumulation**: Each serial read chunk is appended to `partial_buffer_`
|
||||
2. **Line Splitting**: Buffer is searched for newline delimiters
|
||||
3. **Validation**: Each line is checked for JSON structure (presence of `{` and `}`)
|
||||
4. **Sanitization**: Leading garbage (characters before `{`) is stripped
|
||||
5. **Incomplete Message Handling**: If a line lacks a closing brace, it's pushed back to the buffer and the loop waits for more data
|
||||
6. **Parse & Publish**: Complete JSON lines are parsed and published
|
||||
|
||||
**Why This Matters:**
|
||||
- Serial reads may return fragments of a JSON message (e.g., `",\"gyro\":{...}"`)
|
||||
- Multiple messages can arrive in a single read
|
||||
- Buffering ensures robust handling of all edge cases
|
||||
|
||||
---
|
||||
|
||||
## Error Handling & Recovery
|
||||
|
||||
| Scenario | Behavior | Recovery |
|
||||
|----------|----------|----------|
|
||||
| Serial read timeout | Loop continues, checks `reading_` flag | Automatic retry on next iteration |
|
||||
| Incomplete JSON in buffer | Fragment is retained; waits for next read | No action needed; accumulation handles it |
|
||||
| JSON parse error | Error logged; thread continues listening | Move to next message |
|
||||
| Serial device disconnect | readString returns 0; loop continues | Application can reconnect via `open_device()` |
|
||||
| MQTT broker unreachable | Exception caught and logged | Retry via `mqtt_connect()` |
|
||||
| MQTT message error | Exception caught and logged | Connection remains for next message |
|
||||
|
||||
---
|
||||
|
||||
## Thread Safety
|
||||
|
||||
- **Atomic Flag**: `reading_` uses `std::atomic_bool` for lock-free thread signaling
|
||||
- **Publisher Thread-Safety**: rclcpp publishers are thread-safe; `parse_data()` can safely publish from reader thread
|
||||
- **Resource Cleanup**: `stop_read()` joins the thread before returning, ensuring clean shutdown
|
||||
- **No Shared Mutable State**: Aside from `reading_` and the publisher, thread does not access other class members during execution
|
||||
|
||||
---
|
||||
|
||||
## Integration with LifecycleManager
|
||||
|
||||
The `LifecycleManager` orchestrates `HardwareInterface` lifecycle:
|
||||
|
||||
| Lifecycle Phase | LifecycleManager Call | HardwareInterface Action |
|
||||
|---|---|---|
|
||||
| **Configure** | `hw_interface->open_device()` or `mqtt_configure()` | Open serial port or set up MQTT client |
|
||||
| **Activate** | `hw_interface->start_read()` or `mqtt_reader()` | Spawn reader thread or attach MQTT callbacks |
|
||||
| **Deactivate** | `hw_interface->stop_read()` or `close_mqtt_conn()` | Stop reader thread and join; disconnect MQTT |
|
||||
| **Cleanup** | `hw_interface->close_device()` | Release serial port |
|
||||
|
||||
---
|
||||
|
||||
## Usage Example
|
||||
|
||||
### Direct Instantiation (Advanced)
|
||||
|
||||
```cpp
|
||||
// Create an instance (normally managed by LifecycleManager)
|
||||
auto hw = std::make_shared<HardwareInterface>();
|
||||
|
||||
// Serial workflow
|
||||
hw->open_device("/dev/ttyUSB0", 115200);
|
||||
hw->start_read();
|
||||
// ... node spins and publishes IMU data ...
|
||||
hw->stop_read();
|
||||
hw->close_device();
|
||||
|
||||
// MQTT workflow
|
||||
hw->mqtt_configure();
|
||||
hw->mqtt_reader();
|
||||
// ... node spins and publishes IMU data ...
|
||||
hw->close_mqtt_conn();
|
||||
```
|
||||
|
||||
### Via LifecycleManager (Recommended)
|
||||
|
||||
```bash
|
||||
# Launch and manage via lifecycle
|
||||
ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node \
|
||||
--ros-args -p device_path:=/dev/ttyUSB0 -p baudrate:=115200 -p comm_t:=serial
|
||||
|
||||
# Configure and activate
|
||||
ros2 lifecycle set /lifecycle_manager configure
|
||||
ros2 lifecycle set /lifecycle_manager activate
|
||||
|
||||
# Subscribe to IMU data
|
||||
ros2 topic echo /imu_data
|
||||
|
||||
# Deactivate and cleanup
|
||||
ros2 lifecycle set /lifecycle_manager deactivate
|
||||
ros2 lifecycle set /lifecycle_manager shutdown
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Design Patterns
|
||||
|
||||
1. **Abstraction Pattern**: Encapsulates serial and MQTT complexity behind a unified interface
|
||||
2. **Thread Management**: Background reader thread with atomic signaling for clean shutdown
|
||||
3. **Buffer Accumulation**: Handles fragmented reads and multi-message batches robustly
|
||||
4. **Dual Backend Strategy**: Runtime selection of communication mode (serial or MQTT)
|
||||
5. **JSON Deserialization**: Uses industry-standard `nlohmann::json` for robust parsing
|
||||
|
||||
---
|
||||
|
||||
|
||||
## Dependencies
|
||||
|
||||
- **rclcpp**: ROS2 C++ client library
|
||||
- **sensor_msgs**: ROS2 standard sensor message definitions
|
||||
- **paho-mqtt**: Paho C/C++ MQTT client library
|
||||
- **nlohmann/json**: Header-only JSON parsing library
|
||||
- **serialib**: Custom serial communication wrapper
|
||||
|
||||
---
|
||||
|
||||
## Class Diagram
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────┐
|
||||
│ HardwareInterface │
|
||||
│ (rclcpp::Node) │
|
||||
├─────────────────────────────────────┤
|
||||
│ Private Members: │
|
||||
│ - serialib serial │
|
||||
│ - async_client mqtt_client │
|
||||
│ - callback mqtt_cb │
|
||||
│ - thread read_thread_ │
|
||||
│ - atomic_bool reading_ │
|
||||
│ - string partial_buffer_ │
|
||||
│ - Publisher imu_publisher │
|
||||
├─────────────────────────────────────┤
|
||||
│ Public Methods: │
|
||||
│ + start_read() │
|
||||
│ + stop_read() │
|
||||
│ + open_device() │
|
||||
│ + close_device() │
|
||||
│ + is_device_open() │
|
||||
│ + write() │
|
||||
│ + mqtt_configure() │
|
||||
│ + mqtt_reader() │
|
||||
│ + mqtt_connect() │
|
||||
│ + close_mqtt_conn() │
|
||||
│ + parse_data() │
|
||||
│ + publish_imu_data() │
|
||||
└─────────────────────────────────────┘
|
||||
↑
|
||||
│ orchestrated by
|
||||
│
|
||||
┌──────────────────┐
|
||||
│ LifecycleManager │
|
||||
└──────────────────┘
|
||||
```
|
||||
223
doc/architecture/nodes/LifecycleManager.md
Normal file
223
doc/architecture/nodes/LifecycleManager.md
Normal file
@@ -0,0 +1,223 @@
|
||||
# LifecycleManager (`assignments::two::g2_2025_lifecycle_node`)
|
||||
|
||||
## Overview
|
||||
The `LifecycleManager` is the core lifecycle-aware node responsible for managing the IMU reader system's operational states and hardware communication. It orchestrates transitions between configuration, activation, and deactivation phases, abstracting the complexity of dual communication backends (serial and MQTT) into a unified interface.
|
||||
|
||||
#### Implementation Details
|
||||
|
||||
**Parameters**
|
||||
|
||||
- **`device_path`** (string, default: "/dev/ttyUSB0"): Serial device path for hardware connection (e.g., USB serial adapter).
|
||||
- **`baudrate`** (int, default: 115200): Serial communication baud rate in bits per second.
|
||||
- **`comm_t`** (string, default: "serial"): Communication type selector—either "serial" or "mqtt" to determine which backend to use.
|
||||
|
||||
**Constructor**
|
||||
```cpp
|
||||
LifecycleManager()
|
||||
```
|
||||
- Initializes ROS2 lifecycle node with name `lifecycle_manager`
|
||||
- Declares and reads configuration parameters: `device_path`, `baudrate`, and `comm_t`
|
||||
- Creates a shared instance of `HardwareInterface` for managing all hardware operations
|
||||
- Logs initialization status and readiness
|
||||
|
||||
**Core Functions**
|
||||
|
||||
**`CallbackReturn on_configure(const State&)`**
|
||||
- Enters the *Unconfigured* → *Inactive* transition
|
||||
- Checks the `comm_t` parameter to route initialization:
|
||||
- **MQTT mode**: Calls `hw_interface->mqtt_configure()` to set up the MQTT client and broker connection
|
||||
- **Serial mode**: Calls `hw_interface->open_device(device_path_, baudrate_)` to open and configure the serial port
|
||||
- Returns `SUCCESS` if device initialization succeeds, `FAILURE` if serial/MQTT setup fails
|
||||
- Logs configuration status and any errors
|
||||
- Example test code (currently commented) demonstrates direct JSON parsing for validation
|
||||
|
||||
**`CallbackReturn on_activate(const State&)`**
|
||||
- Enters the *Inactive* → *Active* transition
|
||||
- Checks the `comm_t` parameter to start the appropriate reader:
|
||||
- **MQTT mode**: Calls `hw_interface->mqtt_reader()` to attach MQTT callbacks and begin receiving messages
|
||||
- **Serial mode**: Calls `hw_interface->start_read()` to spawn a background thread that continuously reads from the serial device
|
||||
- Returns `SUCCESS` after reader startup
|
||||
- Logs activation status and selected communication type
|
||||
|
||||
**`CallbackReturn on_deactivate(const State&)`**
|
||||
- Enters the *Active* → *Inactive* transition
|
||||
- Checks the `comm_t` parameter to cleanly stop operations:
|
||||
- **MQTT mode**: Calls `hw_interface->close_mqtt_conn()` to disconnect from the broker and clean up resources
|
||||
- **Serial mode**:
|
||||
- Verifies device state with `hw_interface->is_device_open()`
|
||||
- Calls `hw_interface->stop_read()` to signal the reader thread to exit and joins it
|
||||
- Calls `hw_interface->close_device()` to release the serial port
|
||||
- Returns `SUCCESS` after cleanup completes
|
||||
- Logs deactivation and resource release
|
||||
|
||||
**`CallbackReturn on_shutdown(const State&)`**
|
||||
- Enters the *Inactive* → *Finalized* transition
|
||||
- Performs final shutdown logging
|
||||
- Returns `SUCCESS`
|
||||
|
||||
**`CallbackReturn on_cleanup(const State&)`**
|
||||
- Called during error recovery or explicit cleanup commands
|
||||
- Performs resource cleanup and state logging
|
||||
- Returns `SUCCESS`
|
||||
|
||||
|
||||
## Communication Architecture
|
||||
|
||||
### Dual Backend Support
|
||||
|
||||
The `LifecycleManager` provides a flexible, pluggable communication architecture via the `comm_t` parameter:
|
||||
|
||||
#### Serial Communication Path
|
||||
1. **Configuration Phase** (`on_configure`):
|
||||
- Opens the serial device at the path specified by `device_path` and baudrate
|
||||
- Validates device readiness
|
||||
|
||||
2. **Activation Phase** (`on_activate`):
|
||||
- Spawns a background reader thread via `hw_interface->start_read()`
|
||||
- Thread continuously polls the serial device with a timeout
|
||||
- Reads are accumulated in a partial buffer, split on newline, and parsed as JSON
|
||||
- Each valid JSON IMU payload is parsed into a `sensor_msgs::msg::Imu` and published to the ROS topic `imu/data`
|
||||
|
||||
3. **Deactivation Phase** (`on_deactivate`):
|
||||
- Signals the reader thread to stop via atomic flag
|
||||
- Joins the thread to ensure clean termination
|
||||
- Closes the serial device
|
||||
|
||||
#### MQTT Communication Path
|
||||
1. **Configuration Phase** (`on_configure`):
|
||||
- Creates a persistent MQTT async client pointing to the broker at `SERVER_ADDRESS` (default: `tcp://localhost:1883`)
|
||||
- Initializes MQTT callback infrastructure
|
||||
|
||||
2. **Activation Phase** (`on_activate`):
|
||||
- Attaches MQTT callbacks to the client
|
||||
- Subscribes to the topic specified by `TOPIC` (default: `esp32/imu`)
|
||||
- The async client runs background threads to receive messages
|
||||
|
||||
3. **Deactivation Phase** (`on_deactivate`):
|
||||
- Disconnects from the broker
|
||||
- Cleans up MQTT client and callback resources
|
||||
|
||||
---
|
||||
|
||||
## Lifecycle Commands
|
||||
|
||||
To interact with the `LifecycleManager` from the command line, use the following ROS2 lifecycle service calls:
|
||||
|
||||
```bash
|
||||
# List current lifecycle state
|
||||
ros2 lifecycle list /lifecycle_manager
|
||||
|
||||
# Transition: UNCONFIGURED -> INACTIVE
|
||||
ros2 lifecycle set /lifecycle_manager configure
|
||||
|
||||
# Transition: INACTIVE -> ACTIVE
|
||||
ros2 lifecycle set /lifecycle_manager activate
|
||||
|
||||
# Transition: ACTIVE -> INACTIVE
|
||||
ros2 lifecycle set /lifecycle_manager deactivate
|
||||
|
||||
# Transition: INACTIVE -> FINALIZED
|
||||
ros2 lifecycle set /lifecycle_manager shutdown
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Data Flow
|
||||
|
||||
### Serial Data Flow
|
||||
```
|
||||
Hardware Device
|
||||
↓
|
||||
Serial Port (/dev/ttyUSB0)
|
||||
↓
|
||||
Background Reader Thread (start_read)
|
||||
↓
|
||||
Partial Buffer Accumulation
|
||||
↓
|
||||
JSON Line Extraction & Sanitization
|
||||
↓
|
||||
parse_data() ← Deserializes JSON to sensor_msgs::msg::Imu
|
||||
↓
|
||||
imu_publisher → ROS2 Topic (`imu_data`)
|
||||
```
|
||||
|
||||
### MQTT Data Flow
|
||||
```
|
||||
MQTT Broker (localhost:1883)
|
||||
↓
|
||||
MQTT Async Client (Background Thread)
|
||||
↓
|
||||
Subscription to Topic (esp32/imu)
|
||||
↓
|
||||
MQTT Callback Handler
|
||||
↓
|
||||
parse_data() ← Deserializes JSON to sensor_msgs::msg::Imu
|
||||
↓
|
||||
imu_publisher → ROS2 Topic (`imu_data`)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Error Handling
|
||||
|
||||
- **Serial Device Failures**: If `open_device()` fails during configuration, `on_configure()` returns `FAILURE` and the system remains in the `UNCONFIGURED` state
|
||||
- **Communication Errors**: JSON parse errors from invalid payloads are caught and logged without crashing the node; the reader continues listening for the next message
|
||||
- **Thread Safety**: The reader thread uses an atomic flag (`reading_`) for clean stop signaling and ensures all resources are properly joined before returning from `on_deactivate()`
|
||||
|
||||
---
|
||||
|
||||
## Design Patterns
|
||||
|
||||
1. **Strategy Pattern**: The `comm_t` parameter enables runtime selection of communication backend without changing node code
|
||||
2. **Lifecycle Pattern**: Follows ROS2 managed node pattern for predictable initialization, startup, and shutdown sequences
|
||||
3. **Thread Safety**: Atomic flags and resource cleanup ensure the reader thread can be safely started and stopped
|
||||
4. **Buffer Accumulation**: Partial message buffering handles fragmented serial reads and ensures complete JSON objects are parsed
|
||||
|
||||
---
|
||||
|
||||
## Integration with HardwareInterface
|
||||
|
||||
The `LifecycleManager` delegates all hardware operations to the `HardwareInterface` class:
|
||||
|
||||
| Operation | Method | Lifecycle Phase |
|
||||
|-----------|--------|-----------------|
|
||||
| Open serial device | `open_device(path, baud)` | on_configure |
|
||||
| Start reading | `start_read()` | on_activate |
|
||||
| Stop reading | `stop_read()` | on_deactivate |
|
||||
| Close serial device | `close_device()` | on_deactivate |
|
||||
| Configure MQTT | `mqtt_configure()` | on_configure |
|
||||
| Start MQTT reading | `mqtt_reader()` | on_activate |
|
||||
| Close MQTT connection | `close_mqtt_conn()` | on_deactivate |
|
||||
| Parse JSON payload | `parse_data(json_string)` | on_activate (continuous) |
|
||||
| Publish IMU message | `publish_imu_data(imu_msg)` | on_activate (continuous) |
|
||||
|
||||
---
|
||||
|
||||
## Usage Example
|
||||
|
||||
```bash
|
||||
# Launch the node with serial communication at /dev/ttyUSB0, 115200 baud
|
||||
ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node \
|
||||
--ros-args \
|
||||
-p device_path:=/dev/ttyUSB0 \
|
||||
-p baudrate:=115200 \
|
||||
-p comm_t:=serial
|
||||
|
||||
# Alternatively, launch with MQTT communication
|
||||
ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node \
|
||||
--ros-args \
|
||||
-p comm_t:=mqtt
|
||||
|
||||
# In another terminal, configure and activate the lifecycle
|
||||
ros2 lifecycle set /lifecycle_manager configure
|
||||
ros2 lifecycle set /lifecycle_manager activate
|
||||
|
||||
# Subscribe to published IMU data
|
||||
ros2 topic echo /imu_data
|
||||
|
||||
# Deactivate and shutdown
|
||||
ros2 lifecycle set /lifecycle_manager deactivate
|
||||
ros2 lifecycle set /lifecycle_manager shutdown
|
||||
```
|
||||
|
||||
---
|
||||
Reference in New Issue
Block a user