diff --git a/doc/architecture/nodes/DatabaseHandlerNode.md b/doc/architecture/nodes/DatabaseHandlerNode.md new file mode 100644 index 0000000..d2e54fe --- /dev/null +++ b/doc/architecture/nodes/DatabaseHandlerNode.md @@ -0,0 +1,41 @@ +# DatabaseHandlerNode (`assignments::three::g2_2025_database_node`) + +The `DatabaseHandlerNode` subscribes to position and velocity topics and stores the data in a +PostgreSQL database via the `DatabaseManager`. + +## Implementation Details + +**Parameters** + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `save_position_data` | bool | true | Enable storage of position | +| `save_velocity_data` | bool | true | Enable storage of velocity | + +**Constructor** +```cpp +DatabaseHandlerNode() +``` +- Initializes ROS2 node with name `database_handler_node` +- Creates subscriptions to `estimated_position` and/or `estimated_velocity` based on parameter configuration + +## Functions + +**`void position_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg)`** +- Primary callback invoked whenever a position message is received +- Forwards: x, y, and theta to `DatabaseManager::store_position_data` +- Throttled warning (5 second interval) on storage failures + +**`void velocity_callback(const geometry_msgs::msg::Twist::SharedPtr msg)`** +- Primary callback invoked whenever a velocity message is received +- Forwards: linear velocity (x, y, z) and angular velocity (z) to `DatabaseManager::store_velocity_data` +- Throttled warning (5 second interval) on storage failures + +## ROS2 Interface + +**Subscriptions** +- `estimated_position` (geometry_msgs/msg/Pose2D) + - Receives calculated position estimates (only subscribed if `save_position_data=true`) + +- `estimated_velocity` (geometry_msgs/msg/Twist) + - Receives calculated velocity estimates (only subscribed if `save_velocity_data=true`) diff --git a/doc/architecture/nodes/IMUPositionApproximator.md b/doc/architecture/nodes/IMUPositionApproximator.md new file mode 100644 index 0000000..adbb4f7 --- /dev/null +++ b/doc/architecture/nodes/IMUPositionApproximator.md @@ -0,0 +1,58 @@ +# IMUPositionApproximator (`assignments::three::g2_2025_imu_position_approximator_node`) + +The `IMUPositionApproximator` node calculates position and velocity estimates by integrating +acceleration data from an IMU sensor. It transforms sensor data from the robot frame to the +map frame, accounting for rotations and centripetal effects, and provides a position reset +topic. + +## Implementation Details + +**Parameters** + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `initial_x` | double | 0.0 | Initial x position in map frame | +| `initial_y` | double | 0.0 | Initial y position in map frame | +| `initial_z` | double | 0.0 | Initial z position in map frame | +| `initial_theta` | double | 0.0 | Initial orientation angle (radians) | +| `imu_topic` | string | "imu_data" | Topic name for IMU sensor data | + +**Constructor** +```cpp +IMUPositionApproximator() +``` +- Initializes ROS2 node with name `imu_position_approximator` +- Creates subscription to IMU topic (configurable) and `position_reset` topic +- Creates publishers for `estimated_position` and `estimated_velocity` + +## Functions + +**`void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)`** +- Primary callback invoked whenever an IMU message is received +- Calculates time delta `dt` between measurements; skips invalid deltas (≤0 or >1 second) +- Updates orientation: `theta += omega_z * dt` and normalizes to [-pi, pi] +- Applies gravity compensation: subtracts 9.81 m/s^2 from z-axis acceleration +- Transforms acceleration from robot frame to map frame using rotation matrix +- Corrects for centripetal acceleration due to rotation +- Integrates acceleration to update velocity, then integrates velocity to update position +- Publishes `estimated_position` (Pose2D) and `estimated_velocity` (Twist) + +**`void position_reset_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg)`** +- Updates position (x, y) and orientation (theta) to provided values +- Resets all velocities (vx, vy, vz) to zero + +## ROS2 Interface + +**Subscribers** +- `imu_data` (sensor_msgs/msg/Imu) [configurable via `imu_topic` parameter] + - Receives raw IMU sensor data with linear acceleration and angular velocity + +- `position_reset` (geometry_msgs/msg/Pose2D) + - Receives position reset commands from external position determinator + +**Publishers** +- `estimated_position` (geometry_msgs/msg/Pose2D) + - Publishes calculated position (x, y, theta) in map frame + +- `estimated_velocity` (geometry_msgs/msg/Twist) + - Publishes calculated linear velocities (x, y, z) and angular velocity (z) diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index df03755..f3b7399 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/src/g2_2025_odometry_pkg/CMakeLists.txt @@ -60,7 +60,6 @@ ament_target_dependencies(database_handler_node geometry_msgs ) -# Install the executable install(TARGETS position_speed_approximator_node database_handler_node