feat(imu_position_approximator): First approximator implementation

src/g2_2025_odometry_pkg/CMakeLists.txt

@@ -7,9 +7,32 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+# Add executable for position_speed_approximator_node
+add_executable(position_speed_approximator_node
+  src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.cpp
+  src/g2_2025_imu_position_approximator_node/Main.cpp
+)
+
+target_include_directories(position_speed_approximator_node PRIVATE
+  ${CMAKE_CURRENT_SOURCE_DIR}/src
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_position_approximator_node
+)
+
+ament_target_dependencies(position_speed_approximator_node
+  rclcpp
+  sensor_msgs
+  geometry_msgs
+)
+
+# Install the executable
+install(TARGETS
+  position_speed_approximator_node
+  DESTINATION lib/${PROJECT_NAME}
+)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)

src/g2_2025_odometry_pkg/package.xml

@@ -9,6 +9,10 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>

src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/Main.cpp

@@ -0,0 +1,14 @@
+#include "rclcpp/rclcpp.hpp"
+
+#include "nodes/IMUPositionApproximator.hpp"
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+
+    auto node = std::make_shared<assignments::three::g2_2025_imu_position_approximator_node::IMUPositionApproximator>();
+
+    rclcpp::spin(node->get_node_base_interface());
+    rclcpp::shutdown();
+
+    return 0;
+}

src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.cpp

@@ -0,0 +1,126 @@
+#include "IMUPositionApproximator.hpp"
+
+namespace assignments::three::g2_2025_imu_position_approximator_node {
+
+IMUPositionApproximator::IMUPositionApproximator()
+: Node("imu_position_approximator")
+, x_(0.0), y_(0.0), z_(0.0)
+, theta_(0.0)
+, vx_(0.0), vy_(0.0), vz_(0.0)
+, last_time_(this->now())
+{
+    RCLCPP_INFO(this->get_logger(), "IMU Position Approximator node started");
+
+    // Get initial position from parameters
+    x_ = this->declare_parameter<double>("initial_x", 0.0);
+    y_ = this->declare_parameter<double>("initial_y", 0.0);
+    z_ = this->declare_parameter<double>("initial_z", 0.0);
+    theta_ = this->declare_parameter<double>("initial_theta", 0.0);
+    imu_topic_ = this->declare_parameter<std::string>("imu_topic", "imu_data");
+
+    RCLCPP_INFO(this->get_logger(),
+        "Initial position set to: x=%.3f, y=%.3f, z=%.3f, theta=%.3f rad",
+        x_, y_, z_, theta_);
+
+
+    imu_subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
+        imu_topic_, 10,
+        std::bind(&IMUPositionApproximator::imu_callback, this, std::placeholders::_1)
+    );
+
+    position_reset_subscription_ = this->create_subscription<geometry_msgs::msg::Pose2D>(
+        "position_reset", 10,
+        std::bind(&IMUPositionApproximator::position_reset_callback, this, std::placeholders::_1)
+    );
+
+    RCLCPP_INFO(this->get_logger(), "Subscribed to '%s' and 'position_reset'", imu_topic_.c_str());
+
+    position_publisher_ = this->create_publisher<geometry_msgs::msg::Pose2D>("estimated_position", 10);
+    velocity_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("estimated_velocity", 10);
+
+    RCLCPP_INFO(this->get_logger(), "Publishing to 'estimated_position' and 'estimated_velocity'");
+}
+
+void IMUPositionApproximator::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
+    auto current_time = this->now();
+    double dt = (current_time - last_time_).seconds();
+    last_time_ = current_time;
+
+    if (dt <= 0.0 || dt > 1.0) {
+        RCLCPP_WARN(this->get_logger(), "skipping iteration due to invalid delta time (dt=%.3f)", dt);
+        return;
+    }
+
+    Position pos_robot;
+    pos_robot.x = msg->linear_acceleration.x;
+    pos_robot.y = msg->linear_acceleration.y;
+    // remove gravity, standard gravity is ~9.81 m/s^2
+    pos_robot.z = msg->linear_acceleration.z - 9.81;
+
+    // calculate and normalize theta_ to be within [-pi, pi]
+    double omega_z = msg->angular_velocity.z;
+    theta_ += omega_z * dt;
+
+    while (theta_ > M_PI) theta_ -= 2.0 * M_PI;
+    while (theta_ < -M_PI) theta_ += 2.0 * M_PI;
+
+    // transform from robot frame to map frame
+    Position pos_map;
+    double cos_theta = std::cos(theta_);
+    double sin_theta = std::sin(theta_);
+    pos_map.x = pos_robot.x * cos_theta - pos_robot.y * sin_theta;
+    pos_map.y = pos_robot.x * sin_theta + pos_robot.y * cos_theta;
+    pos_map.z = pos_robot.z;
+
+    // account for centripetal acceleration due to rotation when rotating,
+    // linear velocity creates centripetal acceleration
+    pos_map.x -= -omega_z * vy_;
+    pos_map.y -= omega_z * vx_;
+
+    // integrate acceleration to get velocity
+    vx_ += pos_map.x * dt;
+    vy_ += pos_map.y * dt;
+    vz_ += pos_map.z * dt;
+
+    // integrate velocity to get position
+    x_ += vx_ * dt;
+    y_ += vy_ * dt;
+    z_ += vz_ * dt;
+
+    auto position_msg = geometry_msgs::msg::Pose2D();
+    position_msg.x = x_;
+    position_msg.y = y_;
+    position_msg.theta = theta_;
+    position_publisher_->publish(position_msg);
+
+    auto velocity_msg = geometry_msgs::msg::Twist();
+    velocity_msg.linear.x = vx_;
+    velocity_msg.linear.y = vy_;
+    velocity_msg.linear.z = vz_;
+    velocity_msg.angular.z = omega_z;
+    velocity_publisher_->publish(velocity_msg);
+
+    RCLCPP_INFO(this->get_logger(),
+        "Position: (%.3f, %.3f, %.3f) m, θ=%.3f rad | Velocity: (%.3f, %.3f, %.3f) m/s",
+        x_, y_, z_, theta_, vx_, vy_, vz_
+    );
+}
+
+void IMUPositionApproximator::position_reset_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg) {
+    RCLCPP_INFO(this->get_logger(),
+        "resetting position, was: (%.3f, %.3f, %.3f), now: (%.3f, %.3f, %.3f)",
+        x_, y_, theta_, msg->x, msg->y, msg->theta
+    );
+
+    x_ = msg->x;
+    y_ = msg->y;
+    theta_ = msg->theta;
+
+    vx_ = 0.0;
+    vy_ = 0.0;
+    vz_ = 0.0;
+
+    last_time_ = this->now();
+}
+
+} // namespace assignments::three::g2_2025_imu_position_approximator_node

src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.hpp

@@ -0,0 +1,36 @@
+#include <cmath>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace assignments::three::g2_2025_imu_position_approximator_node {
+
+struct Position {
+    double x;
+    double y;
+    double z;
+};
+
+class IMUPositionApproximator : public rclcpp::Node {
+public:
+    IMUPositionApproximator();
+
+private:
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;
+    rclcpp::Subscription<geometry_msgs::msg::Pose2D>::SharedPtr position_reset_subscription_;
+    rclcpp::Publisher<geometry_msgs::msg::Pose2D>::SharedPtr position_publisher_;
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher_;
+
+    std::string imu_topic_ { "imu_data" };
+    double x_, y_, z_;      // Position
+    double theta_;          // Orientation (yaw angle)
+    double vx_, vy_, vz_;   // Velocity
+    rclcpp::Time last_time_;
+
+    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg);
+    void position_reset_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg);
+};
+
+} // namespace assignments::three::g2_2025_imu_position_approximator_node