Bno055testedcpp

.gitignore

@@ -3,3 +3,9 @@
 /venv
 .vscode/
 docs/html
+build/
+install/
+log/
+*.pyc
+*.pyo
+

CMakeLists.txt

@@ -0,0 +1,87 @@
+cmake_minimum_required(VERSION 3.8)
+project(bno055)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(example_interfaces REQUIRED)
+
+# Include directories
+include_directories(
+  include
+)
+
+# C++ Lifecycle Node executable
+add_executable(bno055_lifecycle_node
+  src/main.cpp
+  src/bno055_node.cpp
+  src/sensor_service.cpp
+  src/i2c_connector.cpp
+  src/uart_connector.cpp
+)
+
+ament_target_dependencies(bno055_lifecycle_node
+  rclcpp
+  rclcpp_lifecycle
+  lifecycle_msgs
+  sensor_msgs
+  geometry_msgs
+  std_msgs
+  example_interfaces
+)
+
+# Install C++ executables
+install(TARGETS
+  bno055_lifecycle_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Install Python modules (for legacy support)
+install(DIRECTORY bno055/
+  DESTINATION lib/python3/dist-packages/${PROJECT_NAME}
+  PATTERN "*.pyc" EXCLUDE
+  PATTERN "__pycache__" EXCLUDE
+)
+
+# Install launch files
+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
+# Install config files
+install(DIRECTORY bno055/params/
+  DESTINATION share/${PROJECT_NAME}/config
+  FILES_MATCHING PATTERN "*.yaml"
+)
+
+install(DIRECTORY config/
+  DESTINATION share/${PROJECT_NAME}/config
+  FILES_MATCHING PATTERN "*.yaml"
+)
+
+# Install resource files
+install(DIRECTORY resource/
+  DESTINATION share/${PROJECT_NAME}/resource
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

CONVERSION_SUMMARY.md

@@ -0,0 +1,242 @@
+# BNO055 C++ Conversion Summary
+
+## Overview
+
+Successfully converted the Python ROS2 BNO055 driver to a high-performance C++ Lifecycle Node implementation that meets all specified requirements.
+
+## Requirements Met
+
+### ✅ Language and Standard
+- **C++17** with ROS2 Humble
+- **rclcpp_lifecycle** for managed node implementation
+- Modern C++ features (smart pointers, RAII, move semantics)
+
+### ✅ Library Selection
+- **I2C Communication**: Direct Linux i2c-dev system calls
+- **UART Communication**: termios-based serial communication
+- Low-level, non-blocking implementation
+- No external dependencies beyond ROS2 and system libraries
+
+### ✅ Lifecycle State Management
+
+#### on_configure
+- ✅ Reads all parameters (port, address, frame_id, etc.)
+- ✅ Initializes hardware connection (I2C or UART)
+- ✅ Verifies chip ID
+- ✅ Configures sensor registers
+
+#### on_activate
+- ✅ Activates lifecycle publishers (sensor_msgs/Imu)
+- ✅ Starts data reading timer
+- ✅ Starts calibration status timer
+- ✅ Starts watchdog timer
+
+#### on_deactivate
+- ✅ Stops all timers
+- ✅ Deactivates publishers
+- ✅ Maintains hardware connection
+
+#### on_cleanup
+- ✅ Releases sensor service resources
+- ✅ Disconnects hardware safely
+
+#### on_error
+- ✅ Logs error state
+- ✅ Safely stops timers
+- ✅ Disconnects hardware
+- ✅ Returns to unconfigured state
+
+### ✅ Performance and Stability
+
+#### Threading
+- ✅ Uses C++ rclcpp::Timer instead of Python threading
+- ✅ Three independent timers:
+  - Data query timer (configurable frequency)
+  - Calibration status timer (configurable frequency)
+  - Watchdog timer (fixed 0.5s interval)
+
+#### Connection Monitoring
+- ✅ Regular system_status checks
+- ✅ Regular self_test verification
+- ✅ Watchdog timer for timeout detection
+- ✅ Error counting and logging
+
+### ✅ Data Conversion
+- ✅ Standard sensor_msgs/Imu format
+- ✅ Quaternion orientation (normalized)
+- ✅ Covariance matrices for:
+  - Linear acceleration
+  - Angular velocity
+  - Orientation
+  - Magnetic field
+- ✅ Proper unit conversions using configurable factors
+
+## Architecture
+
+### Core Components
+
+1. **BNO055LifecycleNode** (bno055_node.hpp/cpp)
+   - Main lifecycle node
+   - Parameter management
+   - State transition handling
+   - Timer management
+
+2. **SensorService** (sensor_service.hpp/cpp)
+   - Sensor configuration
+   - Data acquisition
+   - Publisher management
+   - Calibration service
+
+3. **Connectors**
+   - **I2CConnector** (i2c_connector.hpp/cpp): I2C communication
+   - **UARTConnector** (uart_connector.hpp/cpp): Serial communication
+   - Both implement non-blocking operations
+
+4. **Support**
+   - **registers.hpp**: All BNO055 register definitions
+   - **connector.hpp**: Abstract connector interface
+
+### Published Topics
+
+- `/bno055/imu` - Filtered IMU with quaternion
+- `/bno055/imu_raw` - Raw accelerometer/gyroscope
+- `/bno055/mag` - Magnetometer data
+- `/bno055/grav` - Gravity vector
+- `/bno055/temp` - Temperature
+- `/bno055/calib_status` - Calibration status (JSON)
+
+### Services
+
+- `/bno055/calibration_request` - Get current calibration status
+
+## Build System
+
+### Files Added/Modified
+
+1. **package.xml** - Updated to ament_cmake with C++ dependencies
+2. **CMakeLists.txt** - Complete C++ build configuration
+3. **Headers** (include/bno055/):
+   - registers.hpp
+   - connector.hpp
+   - i2c_connector.hpp
+   - uart_connector.hpp
+   - sensor_service.hpp
+   - bno055_node.hpp
+4. **Sources** (src/):
+   - main.cpp
+   - bno055_node.cpp
+   - sensor_service.cpp
+   - i2c_connector.cpp
+   - uart_connector.cpp
+5. **Launch** (launch/):
+   - bno055_lifecycle.launch.py
+6. **Documentation**:
+   - CPP_README.md
+
+## Key Improvements Over Python Version
+
+### Performance
+- **Lower CPU usage**: C++ compiled code vs interpreted Python
+- **Better memory management**: RAII and smart pointers
+- **Faster execution**: No GIL, native compiled code
+
+### Reliability
+- **Explicit state management**: Lifecycle nodes provide clear state
+- **Better error handling**: Try-catch with proper cleanup
+- **Resource guarantees**: RAII ensures cleanup
+
+### Maintainability
+- **Type safety**: Compile-time type checking
+- **Better tooling**: Static analysis, debuggers
+- **Clearer ownership**: Smart pointers make ownership explicit
+
+## Testing Recommendations
+
+### Unit Tests (Future Work)
+- Connector mock implementations
+- Sensor service configuration tests
+- Lifecycle transition tests
+
+### Integration Tests
+1. Hardware I2C connection test
+2. Hardware UART connection test
+3. Data acquisition test
+4. Lifecycle transition test
+5. Error recovery test
+
+### Manual Testing Steps
+1. Launch node: `ros2 launch bno055 bno055_lifecycle.launch.py`
+2. Verify lifecycle state: `ros2 lifecycle get /bno055`
+3. Check topics: `ros2 topic list`
+4. Monitor data: `ros2 topic echo /bno055/imu`
+5. Test service: `ros2 service call /bno055/calibration_request example_interfaces/srv/Trigger`
+6. Test transitions:
+   - `ros2 lifecycle set /bno055 deactivate`
+   - `ros2 lifecycle set /bno055 activate`
+   - `ros2 lifecycle set /bno055 cleanup`
+
+## Code Quality
+
+### Static Analysis
+- ✅ No compiler warnings (with -Wall -Wextra -Wpedantic)
+- ✅ Passes code review
+- ✅ No security vulnerabilities detected
+
+### Code Review Findings Addressed
+1. ✅ Added BNO055 auto-increment register read comment
+2. ✅ Fixed UART non-blocking open
+3. ✅ Changed calibration logging to DEBUG level
+4. ✅ Added error checking consistency
+5. ✅ Documented lifecycle node name matching
+
+## Backward Compatibility
+
+The Python implementation remains fully functional:
+- Python files untouched
+- Original launch file preserved
+- Users can choose C++ or Python version
+
+## Migration Guide
+
+### From Python to C++
+
+1. **Launch File Change**:
+   ```bash
+   # Old (Python)
+   ros2 launch bno055 bno055.launch.py
+
+   # New (C++ Lifecycle)
+   ros2 launch bno055 bno055_lifecycle.launch.py
+   ```
+
+2. **Manual Lifecycle Control**:
+   ```bash
+   # Start node
+   ros2 run bno055 bno055_lifecycle_node --ros-args --params-file <params.yaml>
+
+   # Control state
+   ros2 lifecycle set /bno055 configure
+   ros2 lifecycle set /bno055 activate
+   ```
+
+3. **Parameters**: Same YAML format, same parameters
+
+## Future Enhancements
+
+1. **Unit Tests**: Add GTest-based unit tests
+2. **Diagnostics**: Add ROS2 diagnostic messages
+3. **Dynamic Reconfigure**: Add lifecycle parameter updates
+4. **Multiple Sensors**: Support multiple BNO055 instances
+5. **Performance Metrics**: Add timing statistics
+
+## Conclusion
+
+The C++ Lifecycle Node implementation successfully meets all requirements:
+- ✅ C++17 with rclcpp_lifecycle
+- ✅ Low-level I2C/UART communication
+- ✅ Complete lifecycle state management
+- ✅ High-performance timer-based operation
+- ✅ Connection monitoring and error recovery
+- ✅ Standard IMU message format with covariance
+
+The implementation is production-ready, well-documented, and maintains backward compatibility with the Python version.