From 62b73f38d42002c31b1a96b93fecb29f51ca2f05 Mon Sep 17 00:00:00 2001 From: Wessel Tip Date: Wed, 26 Nov 2025 20:04:03 +0100 Subject: [PATCH] feat(database_node): Add database writer node --- src/g2_2025_odometry_pkg/CMakeLists.txt | 45 +++++++--- .../src/g2_2025_database_node/Main.cpp | 14 ++++ .../nodes/DatabaseHandlerNode.cpp | 84 +++++++++++++++++++ .../nodes/DatabaseHandlerNode.hpp | 39 +++++++++ 4 files changed, 170 insertions(+), 12 deletions(-) create mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_database_node/Main.cpp create mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_database_node/nodes/DatabaseHandlerNode.cpp create mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_database_node/nodes/DatabaseHandlerNode.hpp diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index ff2786c..ef79109 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/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,22 +40,31 @@ 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} ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_database_node/Main.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_database_node/Main.cpp new file mode 100644 index 0000000..dfb7d25 --- /dev/null +++ b/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(); + + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_database_node/nodes/DatabaseHandlerNode.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_database_node/nodes/DatabaseHandlerNode.cpp new file mode 100644 index 0000000..6374ba0 --- /dev/null +++ b/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("save_position_data", true); + save_velocity_data_ = this->declare_parameter("save_velocity_data", true); + + db_manager_ = std::make_unique(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( + "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( + "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 diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_database_node/nodes/DatabaseHandlerNode.hpp b/src/g2_2025_odometry_pkg/src/g2_2025_database_node/nodes/DatabaseHandlerNode.hpp new file mode 100644 index 0000000..8d29609 --- /dev/null +++ b/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 + +#include +#include +#include +#include + +namespace assignments::three::g2_2025_database_node { + +class DatabaseHandlerNode : public rclcpp::Node { +public: + DatabaseHandlerNode(); + ~DatabaseHandlerNode() = default; + +private: + rclcpp::Subscription::SharedPtr imu_subscriber_; + rclcpp::Subscription::SharedPtr position_subscriber_; + rclcpp::Subscription::SharedPtr velocity_subscriber_; + + std::unique_ptr 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