feat(database_node): Add database writer node

src/g2_2025_odometry_pkg/CMakeLists.txt

@@ -5,6 +5,18 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+
+# external packages
+include(FetchContent)
+
+FetchContent_Declare(
+    tomlplusplus
+    GIT_REPOSITORY https://github.com/marzer/tomlplusplus.git
+    GIT_TAG v3.4.0
+)
+
+FetchContent_MakeAvailable(tomlplusplus)
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -28,9 +40,30 @@ ament_target_dependencies(position_speed_approximator_node
   geometry_msgs
 )
 
+add_executable(database_handler_node
+  src/g2_2025_database_node/Main.cpp
+  src/g2_2025_database_node/nodes/DatabaseHandlerNode.cpp
+  src/database/DatabaseManager.cpp
+  src/config/ConfigManager.cpp
+)
+
+target_include_directories(database_handler_node PRIVATE
+  ${CMAKE_CURRENT_SOURCE_DIR}/src
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_database_node
+)
+
+target_link_libraries(database_handler_node tomlplusplus::tomlplusplus pqxx pq)
+
+ament_target_dependencies(database_handler_node
+  rclcpp
+  sensor_msgs
+  geometry_msgs
+)
+
 # Install the executable
 install(TARGETS
   position_speed_approximator_node
+  database_handler_node
   DESTINATION lib/${PROJECT_NAME}
 )
 find_package(rclcpp REQUIRED)

src/g2_2025_odometry_pkg/src/g2_2025_database_node/Main.cpp

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

src/g2_2025_odometry_pkg/src/g2_2025_database_node/nodes/DatabaseHandlerNode.cpp

@@ -0,0 +1,84 @@
+#include "DatabaseHandlerNode.hpp"
+
+namespace assignments::three::g2_2025_database_node {
+
+DatabaseHandlerNode::DatabaseHandlerNode(): Node("database_handler_node") {
+    RCLCPP_INFO(this->get_logger(), "database handler Node starting");
+
+    save_position_data_ = this->declare_parameter<bool>("save_position_data", true);
+    save_velocity_data_ = this->declare_parameter<bool>("save_velocity_data", true);
+
+    db_manager_ = std::make_unique<assignments::three::DatabaseManager>(this->get_logger());
+
+    if (!db_manager_->is_connected()) {
+        RCLCPP_ERROR(this->get_logger(), "failed to connect to database");
+        return;
+    }
+
+    if (save_position_data_) {
+        position_subscriber_ = this->create_subscription<geometry_msgs::msg::Pose2D>(
+            "estimated_position", 10,
+            std::bind(
+                &DatabaseHandlerNode::position_callback,
+                this,
+                std::placeholders::_1
+            )
+        );
+
+        RCLCPP_INFO(this->get_logger(), "subscribed to 'estimated_position'");
+    }
+
+    if (save_velocity_data_) {
+        velocity_subscriber_ = this->create_subscription<geometry_msgs::msg::Twist>(
+            "estimated_velocity", 10,
+            std::bind(
+                &DatabaseHandlerNode::velocity_callback,
+                this,
+                std::placeholders::_1
+            )
+        );
+        RCLCPP_INFO(this->get_logger(), "subscribed to 'estimated_velocity'");
+    }
+
+    RCLCPP_INFO(this->get_logger(), "database node ready to receive data on topics");
+}
+
+void DatabaseHandlerNode::position_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg) {
+    if (!db_manager_->is_connected()) {
+        return;
+    }
+
+    bool success = db_manager_->store_position_data(msg->x, msg->y, msg->theta);
+
+    if (!success) {
+        RCLCPP_WARN_THROTTLE(this->get_logger(),
+            *this->get_clock(),
+            5000,
+            "failed to store position data in database"
+        );
+    } else {
+        RCLCPP_DEBUG(this->get_logger(), "Stored position=[%.3f, %.3f, %.3f]", msg->x, msg->y, msg->theta);
+    }
+}
+
+void DatabaseHandlerNode::velocity_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
+    if (!db_manager_->is_connected()) {
+        return;
+    }
+
+    bool success = db_manager_->store_velocity_data(msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.z);
+
+    if (!success) {
+        RCLCPP_WARN_THROTTLE(this->get_logger(),
+            *this->get_clock(),
+            5000,
+            "failed to store velocity data in database"
+        );
+    } else {
+        RCLCPP_DEBUG(this->get_logger(), "Stored velocity=[%.3f, %.3f, %.3f, %.3f]",
+            msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.z
+        );
+    }
+}
+
+} // namespace assignments::three::g2_2025_database_node

src/g2_2025_odometry_pkg/src/g2_2025_database_node/nodes/DatabaseHandlerNode.hpp

@@ -0,0 +1,39 @@
+/* DatabaseHandlerNode.hpp
+ * Database handler node for subscribing to topics and storing data
+ *
+ * Changelog:
+ * [26-11-2025] Created database handler node for position and sensor data storage
+ */
+#pragma once
+
+#include "database/DatabaseManager.hpp"
+
+#include <memory>
+
+#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_database_node {
+
+class DatabaseHandlerNode : public rclcpp::Node {
+public:
+    DatabaseHandlerNode();
+    ~DatabaseHandlerNode() = default;
+
+private:
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
+    rclcpp::Subscription<geometry_msgs::msg::Pose2D>::SharedPtr position_subscriber_;
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr velocity_subscriber_;
+
+    std::unique_ptr<assignments::three::DatabaseManager> db_manager_;
+
+    bool save_position_data_ { true };
+    bool save_velocity_data_ { true };
+
+    void position_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg);
+    void velocity_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
+};
+
+} // namespace assignments::three::g2_2025_database_node