ros2-assignments/doc/architecture/nodes/IMUPositionApproximator.md

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

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)