generated from wessel/boilerplate
59 lines
2.5 KiB
Markdown
59 lines
2.5 KiB
Markdown
# 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)
|