diff --git a/doc/architecture/nodes/DatabaseHandlerNode.md b/doc/architecture/nodes/DatabaseHandlerNode.md new file mode 100644 index 0000000..d2e54fe --- /dev/null +++ b/doc/architecture/nodes/DatabaseHandlerNode.md @@ -0,0 +1,41 @@ +# DatabaseHandlerNode (`assignments::three::g2_2025_database_node`) + +The `DatabaseHandlerNode` subscribes to position and velocity topics and stores the data in a +PostgreSQL database via the `DatabaseManager`. + +## Implementation Details + +**Parameters** + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `save_position_data` | bool | true | Enable storage of position | +| `save_velocity_data` | bool | true | Enable storage of velocity | + +**Constructor** +```cpp +DatabaseHandlerNode() +``` +- Initializes ROS2 node with name `database_handler_node` +- Creates subscriptions to `estimated_position` and/or `estimated_velocity` based on parameter configuration + +## Functions + +**`void position_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg)`** +- Primary callback invoked whenever a position message is received +- Forwards: x, y, and theta to `DatabaseManager::store_position_data` +- Throttled warning (5 second interval) on storage failures + +**`void velocity_callback(const geometry_msgs::msg::Twist::SharedPtr msg)`** +- Primary callback invoked whenever a velocity message is received +- Forwards: linear velocity (x, y, z) and angular velocity (z) to `DatabaseManager::store_velocity_data` +- Throttled warning (5 second interval) on storage failures + +## ROS2 Interface + +**Subscriptions** +- `estimated_position` (geometry_msgs/msg/Pose2D) + - Receives calculated position estimates (only subscribed if `save_position_data=true`) + +- `estimated_velocity` (geometry_msgs/msg/Twist) + - Receives calculated velocity estimates (only subscribed if `save_velocity_data=true`) diff --git a/doc/architecture/nodes/IMUPositionApproximator.md b/doc/architecture/nodes/IMUPositionApproximator.md new file mode 100644 index 0000000..adbb4f7 --- /dev/null +++ b/doc/architecture/nodes/IMUPositionApproximator.md @@ -0,0 +1,58 @@ +# 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) diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index e98170a..f3b7399 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/src/g2_2025_odometry_pkg/CMakeLists.txt @@ -5,11 +5,66 @@ 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) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( 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 +) + +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(TARGETS + position_speed_approximator_node + database_handler_node + DESTINATION lib/${PROJECT_NAME} +) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/src/g2_2025_odometry_pkg/package.xml b/src/g2_2025_odometry_pkg/package.xml index de60dca..afe8611 100644 --- a/src/g2_2025_odometry_pkg/package.xml +++ b/src/g2_2025_odometry_pkg/package.xml @@ -9,6 +9,10 @@ ament_cmake + rclcpp + sensor_msgs + geometry_msgs + ament_lint_auto ament_lint_common ament_cmake_gtest diff --git a/src/g2_2025_odometry_pkg/src/config/ConfigManager.cpp b/src/g2_2025_odometry_pkg/src/config/ConfigManager.cpp new file mode 100644 index 0000000..164cbd4 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/config/ConfigManager.cpp @@ -0,0 +1,112 @@ +#include "ConfigManager.hpp" + +#include +#include + +namespace assignments::three { + +ConfigManager::ConfigManager(rclcpp::Logger logger) + : logger_(logger), loaded_(false) +{ + // Try to auto-load config from default location + std::string config_path = find_config_file(); + if (!config_path.empty()) { + load_config(config_path); + } +} + +bool ConfigManager::load_config(const std::string& config_file_path) { + try { + RCLCPP_INFO(logger_, "[CFG] '%s': loading configuration", config_file_path.c_str()); + + if (!std::filesystem::exists(config_file_path)) { + RCLCPP_ERROR(logger_, "[CFG] '%s': file does not exist", config_file_path.c_str()); + return false; + } + + config_ = toml::parse_file(config_file_path); + loaded_ = true; + + RCLCPP_INFO(logger_, "[CFG] '%s': configuration loaded", config_file_path.c_str()); + return true; + + } catch (const toml::parse_error& e) { + RCLCPP_ERROR(logger_, "[CFG] '%s': failed to parse: %s", config_file_path.c_str(), e.what()); + loaded_ = false; + return false; + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "[CFG] '%s': failed to load: %s", config_file_path.c_str(), e.what()); + loaded_ = false; + return false; + } +} + +std::optional ConfigManager::get_database_config() const { + if (!loaded_ || !config_.has_value()) { + RCLCPP_ERROR(logger_, "[CFG] database configuration not loaded"); + return std::nullopt; + } + + try { + return parse_database_config(config_.value()); + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "[CFG] database configuration failed to parse: %s", e.what()); + return std::nullopt; + } +} + +bool ConfigManager::is_loaded() const { + return loaded_; +} + +std::string ConfigManager::find_config_file() const { + // Look for config file in several locations + for (const auto& path : default_config_paths_) { + if (std::filesystem::exists(path)) { + RCLCPP_INFO(logger_, "[CFG] '%s': found configuration file", path.c_str()); + return path; + } + } + + RCLCPP_WARN(logger_, "[CFG] no configuration file found at default locations"); + return ""; +} + +DatabaseConfig ConfigManager::parse_database_config(const toml::table& config) const { + DatabaseConfig db_config; + + // Parse database section + auto database_section = config["database"]; + if (!database_section) { + throw std::runtime_error("missing [database] section in configuration"); + } + + db_config.host = database_section["host"].value_or("localhost"); + db_config.port = database_section["port"].value_or(5432); + db_config.dbname = database_section["dbname"].value_or("imu_data"); + db_config.user = database_section["user"].value_or("postgres"); + db_config.password = database_section["password"].value_or("postgres"); + db_config.timeout = database_section["timeout"].value_or(30); + db_config.ssl = database_section["ssl"].value_or(false); + + // Parse pool section if present + auto pool_section = database_section["pool"]; + if (pool_section) { + db_config.min_connections = pool_section["min_connections"].value_or(1); + db_config.max_connections = pool_section["max_connections"].value_or(10); + } else { + db_config.min_connections = 1; + db_config.max_connections = 10; + } + + RCLCPP_INFO(logger_, "[CFG] database config parsed - %s:%s@%s:%d", + db_config.user.c_str(), + db_config.dbname.c_str(), + db_config.host.c_str(), + db_config.port + ); + + return db_config; +} + +} // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/src/config/ConfigManager.hpp b/src/g2_2025_odometry_pkg/src/config/ConfigManager.hpp new file mode 100644 index 0000000..0cf5887 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/config/ConfigManager.hpp @@ -0,0 +1,52 @@ +/* ConfigManager.hpp + * Configuration management for the exam result generator + * Uses toml++ library to parse TOML configuration files + * + * Reviewed by: + * Changelog: + * [23-09-2025] Wessel T: Created configuration manager class for TOML config loading + */ +#pragma once + +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include "DatabaseConfig.hpp" + +namespace assignments::three { + +class ConfigManager { +public: + explicit ConfigManager(rclcpp::Logger logger); + ~ConfigManager() = default; + + bool load_config(const std::string& config_file_path); + + std::optional get_database_config() const; + + bool is_loaded() const; + +private: + rclcpp::Logger logger_; + + std::optional config_; + + bool loaded_ { false }; + std::vector default_config_paths_ = { + "config.toml", + "./src/config.toml", + "../config.toml", + "../../config.toml", + "../../../config.toml", + "../../../../config.toml", + "/etc/ros2_grade_calculator/config.toml" + }; + + std::string find_config_file() const; + DatabaseConfig parse_database_config(const toml::table& config) const; +}; + +} // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/src/config/DatabaseConfig.hpp b/src/g2_2025_odometry_pkg/src/config/DatabaseConfig.hpp new file mode 100644 index 0000000..04203e8 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/config/DatabaseConfig.hpp @@ -0,0 +1,46 @@ +/* DatabaseConfig.hpp + * Database configuration structure + * + * Reviewed by: + * Changelog: + * [23-09-2025] Wessel T: Create initial configuration structure + */ +#pragma once + +#include + +namespace assignments::three { + +struct DatabaseConfig { + std::string host; + int port; + std::string dbname; + std::string user; + std::string password; + int timeout; + bool ssl; + + // Pool settings + int min_connections; + int max_connections; + + std::string to_connection_string() const { + std::string conn_str = + "host=" + host + + " port=" + std::to_string(port) + + " dbname=" + dbname + + " user=" + user + + " password=" + password + + " connect_timeout=" + std::to_string(timeout); + + if (ssl) { + conn_str += " sslmode=require"; + } else { + conn_str += " sslmode=disable"; + } + + return conn_str; + } +}; + +} // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/src/database/DatabaseManager.cpp b/src/g2_2025_odometry_pkg/src/database/DatabaseManager.cpp new file mode 100644 index 0000000..4912bf1 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/database/DatabaseManager.cpp @@ -0,0 +1,164 @@ +#include "DatabaseManager.hpp" + +#include +#include "rclcpp/rclcpp.hpp" + +#include "SQLQueries.hpp" +#include "config/ConfigManager.hpp" + +namespace assignments::three { + +DatabaseManager::DatabaseManager(rclcpp::Logger logger) : logger_(logger) { + config_manager_ = std::make_unique(logger_); + init_database(); +} + +bool DatabaseManager::is_connected() const { + return conn_ && conn_->is_open(); +} + +bool DatabaseManager::connect(const std::string& connection_string) { + try { + RCLCPP_INFO(logger_, "[DBS] connecting to PostgreSQL database..."); + + conn_ = std::make_unique(connection_string); + + if (conn_->is_open()) { + RCLCPP_INFO(logger_, "[DBS] '%s': successfully connected to database", conn_->dbname()); + return true; + } else { + RCLCPP_ERROR(logger_, "[DBS] failed to open database connection"); + return false; + } + + } catch (const pqxx::sql_error &e) { + RCLCPP_ERROR(logger_, "[DBS] sql error: %s", e.what()); + return false; + } catch (const std::exception &e) { + RCLCPP_ERROR(logger_, "[DBS] connection error: %s", e.what()); + return false; + } +} + +void DatabaseManager::init_database() { + if (!config_manager_ || !config_manager_->is_loaded()) { + RCLCPP_ERROR(logger_, "[DBS] configuration not loaded, cannot initialize database"); + return; + } + + auto db_config = config_manager_->get_database_config(); + if (!db_config.has_value()) { + RCLCPP_ERROR(logger_, "[DBS] failed to get database configuration"); + return; + } + + std::string connection_string = db_config->to_connection_string(); + + if (connect(connection_string)) { + create_tables(); + } +} + +void DatabaseManager::create_tables() { + if (!conn_ || !conn_->is_open()) return; + + try { + pqxx::work txn(*conn_); + + txn.exec(SQL_CREATE_IMU_DATA_TABLE); + txn.exec(SQL_CREATE_POSITION_DATA_TABLE); + txn.exec(SQL_CREATE_VELOCITY_DATA_TABLE); + + txn.commit(); + + RCLCPP_INFO(logger_, "[DBS] database tables ready"); + + } catch (const pqxx::sql_error &e) { + RCLCPP_ERROR(logger_, "[DBS] CREATE TABLE failed: %s", e.what()); + } catch (const std::exception &e) { + RCLCPP_ERROR(logger_, "[DBS] database error: %s", e.what()); + } +} + +bool DatabaseManager::store_imu_data( + double linear_accel_x, double linear_accel_y, double linear_accel_z, + double angular_vel_x, double angular_vel_y, double angular_vel_z +) { + if (!is_connected()) { + RCLCPP_WARN(logger_, "[DBS] not connected to database"); + return false; + } + + try { + pqxx::work txn(*conn_); + + txn.exec_params(SQL_INSERT_IMU_DATA, + linear_accel_x, linear_accel_y, linear_accel_z, + angular_vel_x, angular_vel_y, angular_vel_z + ); + + txn.commit(); + + RCLCPP_DEBUG(logger_, "[DBS] stored IMU data: accel=[%.3f,%.3f,%.3f], angular=[%.3f,%.3f,%.3f]", + linear_accel_x, linear_accel_y, linear_accel_z, + angular_vel_x, angular_vel_y, angular_vel_z + ); + + return true; + + } catch (const pqxx::sql_error &e) { + RCLCPP_ERROR(logger_, "[DBS] failed to store IMU data: %s", e.what()); + return false; + } catch (const std::exception &e) { + RCLCPP_ERROR(logger_, "[DBS] database error: %s", e.what()); + return false; + } +} + +bool DatabaseManager::store_position_data(double x, double y, double theta) { + if (!is_connected()) { + RCLCPP_WARN(logger_, "[DBS] not connected to database"); + return false; + } + + try { + pqxx::work txn(*conn_); + txn.exec_params(SQL_INSERT_POSITION_DATA, x, y, theta); + txn.commit(); + + RCLCPP_DEBUG(logger_, "[DBS] stored position data=[%.3f, %.3f, %.3f]", x, y, theta); + + return true; + } catch (const pqxx::sql_error &e) { + RCLCPP_ERROR(logger_, "[DBS] failed to store position data: %s", e.what()); + return false; + } catch (const std::exception &e) { + RCLCPP_ERROR(logger_, "[DBS] database error: %s", e.what()); + return false; + } +} + +bool DatabaseManager::store_velocity_data(double vx, double vy, double vz, double omega_z) { + if (!is_connected()) { + RCLCPP_WARN(logger_, "[DBS] not connected to database"); + return false; + } + + try { + pqxx::work txn(*conn_); + txn.exec_params(SQL_INSERT_VELOCITY_DATA, vx, vy, vz, omega_z); + txn.commit(); + + RCLCPP_DEBUG(logger_, "[DBS] stored velocity data=[%.3f, %.3f, %.3f, %.3f]", vx, vy, vz, omega_z); + + return true; + } catch (const pqxx::sql_error &e) { + RCLCPP_ERROR(logger_, "[DBS] failed to store velocity data: %s", e.what()); + return false; + } catch (const std::exception &e) { + RCLCPP_ERROR(logger_, "[DBS] database error: %s", e.what()); + return false; + } +} + +} // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/src/database/DatabaseManager.hpp b/src/g2_2025_odometry_pkg/src/database/DatabaseManager.hpp new file mode 100644 index 0000000..e6fe8d1 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/database/DatabaseManager.hpp @@ -0,0 +1,48 @@ +/* DatabaseManager.hpp + * Database manager for the IMU data collection system + * + * Reviewed by: + * Changelog: + * [23-09-2025] Wessel T: Created database manager class for all DB operations + * [14-10-2025] Wessel T: Updated for IMU data storage functionality + */ +#pragma once + +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include "config/ConfigManager.hpp" + +namespace assignments::three { + +class DatabaseManager { +public: + explicit DatabaseManager(rclcpp::Logger logger); + ~DatabaseManager() = default; + + bool connect(const std::string& connection_string); + virtual bool is_connected() const; + + // Table operations + virtual void init_database(); + void create_tables(); + + // IMU Data operations + virtual bool store_position_data(double x, double y, double theta); + virtual bool store_velocity_data(double vx, double vy, double vz, double omega_z); + virtual bool store_imu_data( + double linear_accel_x, double linear_accel_y, double linear_accel_z, + double angular_vel_x, double angular_vel_y, double angular_vel_z + ); + +private: + rclcpp::Logger logger_; + + std::unique_ptr conn_; + std::unique_ptr config_manager_; +}; + +} // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/src/database/SQLQueries.hpp b/src/g2_2025_odometry_pkg/src/database/SQLQueries.hpp new file mode 100644 index 0000000..65eb1b0 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/database/SQLQueries.hpp @@ -0,0 +1,61 @@ +/* SQLQueries.hpp + * SQL query definitions for the IMU data collection system + * + * Reviewed by: + * Changelog: + * [23-09-2025] Wessel T: Created initial database state + * [14-10-2025] Wessel T: Updated for IMU data storage + * [26-11-2025] Wessel T: Added position and velocity measurement tables + */ +#pragma once + +static const std::string SQL_CREATE_IMU_DATA_TABLE = R"( + CREATE TABLE IF NOT EXISTS imu_data ( + id SERIAL PRIMARY KEY, + timestamp TIMESTAMP DEFAULT CURRENT_TIMESTAMP, + linear_accel_x REAL NOT NULL, + linear_accel_y REAL NOT NULL, + linear_accel_z REAL NOT NULL, + angular_vel_x REAL NOT NULL, + angular_vel_y REAL NOT NULL, + angular_vel_z REAL NOT NULL + ); +)"; + +static const std::string SQL_CREATE_POSITION_DATA_TABLE = R"( + CREATE TABLE IF NOT EXISTS position_measurements ( + id SERIAL PRIMARY KEY, + timestamp TIMESTAMP DEFAULT CURRENT_TIMESTAMP, + x REAL NOT NULL, + y REAL NOT NULL, + theta REAL NOT NULL + ); +)"; + +static const std::string SQL_CREATE_VELOCITY_DATA_TABLE = R"( + CREATE TABLE IF NOT EXISTS velocity_measurements ( + id SERIAL PRIMARY KEY, + timestamp TIMESTAMP DEFAULT CURRENT_TIMESTAMP, + vx REAL NOT NULL, + vy REAL NOT NULL, + vz REAL NOT NULL, + omega_z REAL NOT NULL + ); +)"; + +static const std::string SQL_INSERT_IMU_DATA = R"( + INSERT INTO imu_data ( + linear_accel_x, linear_accel_y, linear_accel_z, + angular_vel_x, angular_vel_y, angular_vel_z + ) VALUES ($1, $2, $3, $4, $5, $6); +)"; + +static const std::string SQL_INSERT_POSITION_DATA = R"( + INSERT INTO position_measurements (x, y, theta) + VALUES ($1, $2, $3); +)"; + +static const std::string SQL_INSERT_VELOCITY_DATA = R"( + INSERT INTO velocity_measurements (vx, vy, vz, omega_z) + VALUES ($1, $2, $3, $4); +)"; 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 diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/Main.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/Main.cpp new file mode 100644 index 0000000..0609745 --- /dev/null +++ b/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(); + + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.cpp new file mode 100644 index 0000000..e5eecba --- /dev/null +++ b/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("initial_x", 0.0); + y_ = this->declare_parameter("initial_y", 0.0); + z_ = this->declare_parameter("initial_z", 0.0); + theta_ = this->declare_parameter("initial_theta", 0.0); + imu_topic_ = this->declare_parameter("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_subscriber_ = this->create_subscription( + imu_topic_, 10, + std::bind(&IMUPositionApproximator::imu_callback, this, std::placeholders::_1) + ); + + position_reset_subscriber_ = this->create_subscription( + "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("estimated_position", 10); + velocity_publisher_ = this->create_publisher("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 diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.hpp b/src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.hpp new file mode 100644 index 0000000..26ae5b9 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.hpp @@ -0,0 +1,36 @@ +#include + +#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::SharedPtr imu_subscriber_; + rclcpp::Subscription::SharedPtr position_reset_subscriber_; + rclcpp::Publisher::SharedPtr position_publisher_; + rclcpp::Publisher::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