diff --git a/src/hw2_interface_math/CMakeLists.txt b/src/hw2_interface_math/CMakeLists.txt new file mode 100644 index 0000000..5d1d01f --- /dev/null +++ b/src/hw2_interface_math/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.8) +project(hw2_interface_math) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/ServiceVec.srv" +) +ament_export_dependencies(rosidl_default_runtime) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +ament_package() diff --git a/src/hw2_interface_math/package.xml b/src/hw2_interface_math/package.xml new file mode 100644 index 0000000..e08fee0 --- /dev/null +++ b/src/hw2_interface_math/package.xml @@ -0,0 +1,21 @@ + + + + hw2_interface_math + 0.0.0 + TODO: Package description + wessel + TODO: License declaration + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/hw2_interface_math/srv/ServiceVec.srv b/src/hw2_interface_math/srv/ServiceVec.srv new file mode 100644 index 0000000..f2326b3 --- /dev/null +++ b/src/hw2_interface_math/srv/ServiceVec.srv @@ -0,0 +1,19 @@ +# Differential quotient calculation service message +float64 h # start interval +float64 t # value for which df/dt is calculated +int32 n # number of bisect iterations +int32 function_id # standard function identifier +--- +float64 dfdt # approximation of differential quotient + +## format +##primitive message types +# bool +# byte +# char +# float32, float64 +# int8, uint8 +# int16, uint16 +# int32, uint32 +# int64, uint64 +# string diff --git a/src/hw2_service_math/CMakeLists.txt b/src/hw2_service_math/CMakeLists.txt new file mode 100644 index 0000000..bfa0177 --- /dev/null +++ b/src/hw2_service_math/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(hw2_service_math) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(hw2_interface_math REQUIRED) + +add_executable(service_server src/service_server.cpp) +add_executable(service_client src/service_client.cpp) +# add_executable(exercise2_client src/exercise2_client.cpp) +# add_executable(exercise3_client src/exercise3_client.cpp) + +ament_target_dependencies(service_server rclcpp hw2_interface_math) +ament_target_dependencies(service_client rclcpp hw2_interface_math) +# ament_target_dependencies(exercise2_client rclcpp hw2_interface_math) +# ament_target_dependencies(exercise3_client rclcpp hw2_interface_math) + +install ( + TARGETS + service_server + service_client + # exercise2_client + # exercise3_client + 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/hw2_service_math/package.xml b/src/hw2_service_math/package.xml new file mode 100644 index 0000000..2f63f61 --- /dev/null +++ b/src/hw2_service_math/package.xml @@ -0,0 +1,21 @@ + + + + hw2_service_math + 0.0.0 + TODO: Package description + wessel + TODO: License declaration + + ament_cmake + + rclcpp + hw2_interface_math + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/hw2_service_math/src/exercise2_client.cpp b/src/hw2_service_math/src/exercise2_client.cpp new file mode 100644 index 0000000..e01a9ab --- /dev/null +++ b/src/hw2_service_math/src/exercise2_client.cpp @@ -0,0 +1,181 @@ +/* hw2_service_math/exercise2_client.cpp + * Exercise 2: Service client that calculates error between exact and approximated derivatives + * + * This client compares the numerical approximation from the service with the analytical + * (exact) derivative for standard mathematical functions. + * + * Reviewed by: + * Changelog: + * [15-09-2025] Wessel T: Implement exercise 2 client + */ + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include "hw2_interface_math/srv/service_vec.hpp" + +using namespace std::chrono; +using hw2_interface_math::srv::ServiceVec; + +namespace homework::two::service_math { + +class Exercise2Client : public rclcpp::Node { +public: + Exercise2Client() + : Node("exercise2_error_analysis_client"), test_case_(0) + { + client_ = this->create_client("differential_service"); + timer_ = this->create_wall_timer( + seconds(3), + std::bind(&Exercise2Client::run_next_test, this) + ); + + RCLCPP_INFO(this->get_logger(), "Exercise 2 Client started - Error Analysis"); + } + +private: + int test_case_; + + struct TestCase { + double h; + double t; + int n; + int function_id; + std::string function_name; + double exact_derivative; + }; + + // Test cases with known exact derivatives + std::vector test_cases_ = { + // sin(x) -> cos(x) + {0.1, 0.0, 15, 1, "sin(x)", std::cos(0.0)}, // cos(0) = 1 + {0.1, M_PI/2, 15, 1, "sin(x)", std::cos(M_PI/2)}, // cos(π/2) ≈ 0 + {0.1, M_PI, 15, 1, "sin(x)", std::cos(M_PI)}, // cos(π) = -1 + + // cos(x) -> -sin(x) + {0.1, 0.0, 15, 2, "cos(x)", -std::sin(0.0)}, // -sin(0) = 0 + {0.1, M_PI/2, 15, 2, "cos(x)", -std::sin(M_PI/2)}, // -sin(π/2) = -1 + {0.1, M_PI, 15, 2, "cos(x)", -std::sin(M_PI)}, // -sin(π) = 0 + + // exp(x) -> exp(x) + {0.01, 0.0, 20, 4, "exp(x)", std::exp(0.0)}, // exp(0) = 1 + {0.01, 1.0, 20, 4, "exp(x)", std::exp(1.0)}, // exp(1) ≈ 2.718 + {0.01, 2.0, 20, 4, "exp(x)", std::exp(2.0)}, // exp(2) ≈ 7.389 + + // ln(x) -> 1/x + {0.01, 1.0, 20, 5, "ln(x)", 1.0/1.0}, // 1/1 = 1 + {0.01, 2.0, 20, 5, "ln(x)", 1.0/2.0}, // 1/2 = 0.5 + {0.01, 0.5, 20, 5, "ln(x)", 1.0/0.5}, // 1/0.5 = 2 + + // x^2 -> 2x + {0.01, 0.0, 15, 6, "x^2", 2.0*0.0}, // 2*0 = 0 + {0.01, 1.0, 15, 6, "x^2", 2.0*1.0}, // 2*1 = 2 + {0.01, 3.0, 15, 6, "x^2", 2.0*3.0}, // 2*3 = 6 + {0.01, -2.0, 15, 6, "x^2", 2.0*(-2.0)}, // 2*(-2) = -4 + + // x^3 -> 3x^2 + {0.01, 1.0, 15, 7, "x^3", 3.0*1.0*1.0}, // 3*1^2 = 3 + {0.01, 2.0, 15, 7, "x^3", 3.0*2.0*2.0}, // 3*2^2 = 12 + {0.01, -1.0, 15, 7, "x^3", 3.0*(-1.0)*(-1.0)}, // 3*(-1)^2 = 3 + + // sqrt(x) -> 1/(2*sqrt(x)) + {0.01, 1.0, 20, 8, "sqrt(x)", 1.0/(2.0*std::sqrt(1.0))}, // 1/(2*1) = 0.5 + {0.01, 4.0, 20, 8, "sqrt(x)", 1.0/(2.0*std::sqrt(4.0))}, // 1/(2*2) = 0.25 + {0.01, 9.0, 20, 8, "sqrt(x)", 1.0/(2.0*std::sqrt(9.0))}, // 1/(2*3) ≈ 0.167 + + // 1/x -> -1/x^2 + {0.01, 1.0, 20, 9, "1/x", -1.0/(1.0*1.0)}, // -1/1 = -1 + {0.01, 2.0, 20, 9, "1/x", -1.0/(2.0*2.0)}, // -1/4 = -0.25 + {0.01, 0.5, 20, 9, "1/x", -1.0/(0.5*0.5)}, // -1/0.25 = -4 + + // x -> 1 + {0.1, 0.0, 10, 10, "x", 1.0}, // d/dx[x] = 1 + {0.1, 5.0, 10, 10, "x", 1.0}, // d/dx[x] = 1 + {0.1, -3.0, 10, 10, "x", 1.0} // d/dx[x] = 1 + }; + + void run_next_test() { + if (!client_->wait_for_service(seconds(1))) { + RCLCPP_WARN(this->get_logger(), "Service not available"); + return; + } + + if (test_case_ >= test_cases_.size()) { + RCLCPP_INFO(this->get_logger(), "=== Exercise 2 Analysis Complete ==="); + RCLCPP_INFO(this->get_logger(), "All %zu test cases completed.", test_cases_.size()); + rclcpp::shutdown(); + return; + } + + auto& test = test_cases_[test_case_]; + auto request = std::make_shared(); + request->h = test.h; + request->t = test.t; + request->n = test.n; + request->function_id = test.function_id; + + RCLCPP_INFO(this->get_logger(), + "\n=== Test Case %d ===", test_case_ + 1); + RCLCPP_INFO(this->get_logger(), + "Function: %s at t=%.3f", test.function_name.c_str(), test.t); + RCLCPP_INFO(this->get_logger(), + "Parameters: h=%.4f, n=%d", test.h, test.n); + + auto future = client_->async_send_request(request); + auto result = rclcpp::spin_until_future_complete(this->shared_from_this(), future, seconds(5)); + + if (result == rclcpp::FutureReturnCode::SUCCESS) { + auto response = future.get(); + double approximated = response->dfdt; + double exact = test.exact_derivative; + double absolute_error = std::abs(approximated - exact); + double relative_error = (exact != 0.0) ? std::abs(absolute_error / exact) * 100.0 : 0.0; + + RCLCPP_INFO(this->get_logger(), + "Exact derivative: %.8f", exact); + RCLCPP_INFO(this->get_logger(), + "Approximated: %.8f", approximated); + RCLCPP_INFO(this->get_logger(), + "Absolute error: %.2e", absolute_error); + RCLCPP_INFO(this->get_logger(), + "Relative error: %.4f%%", relative_error); + + // Quality assessment + if (relative_error < 0.01) { + RCLCPP_INFO(this->get_logger(), "Quality: EXCELLENT (< 0.01%)"); + } else if (relative_error < 0.1) { + RCLCPP_INFO(this->get_logger(), "Quality: VERY GOOD (< 0.1%)"); + } else if (relative_error < 1.0) { + RCLCPP_INFO(this->get_logger(), "Quality: GOOD (< 1%)"); + } else if (relative_error < 5.0) { + RCLCPP_INFO(this->get_logger(), "Quality: ACCEPTABLE (< 5%)"); + } else { + RCLCPP_WARN(this->get_logger(), "Quality: POOR (>= 5%)"); + } + + } else if (result == rclcpp::FutureReturnCode::TIMEOUT) { + RCLCPP_WARN(this->get_logger(), "Service call timed out"); + } else { + RCLCPP_ERROR(this->get_logger(), "Service call failed"); + } + + test_case_++; + } + + rclcpp::Client::SharedPtr client_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace homework::two::service_math + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/hw2_service_math/src/exercise3_client.cpp b/src/hw2_service_math/src/exercise3_client.cpp new file mode 100644 index 0000000..7792727 --- /dev/null +++ b/src/hw2_service_math/src/exercise3_client.cpp @@ -0,0 +1,313 @@ +/* hw2_service_math/exercise3_client.cpp + * Exercise 3: Service client that calculates derivatives of function combinations + * + * This client demonstrates derivative rules for combinations of functions: + * - Sum rule: (f + g)' = f' + g' + * - Difference rule: (f - g)' = f' - g' + * - Product rule: (f * g)' = f' * g + f * g' + * - Quotient rule: (f / g)' = (f' * g - f * g') / g^2 + * - Chain rule: (f(g(x)))' = f'(g(x)) * g'(x) + * + * Reviewed by: + * Changelog: + * [15-09-2025] Wessel T: Implement exercise 3 client + */ + +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include "hw2_interface_math/srv/service_vec.hpp" + +using namespace std::chrono; +using hw2_interface_math::srv::ServiceVec; + +namespace homework::two::service_math { + +class Exercise3Client : public rclcpp::Node { +public: + Exercise3Client() + : Node("exercise3_combination_client"), test_case_(0) + { + client_ = this->create_client("differential_service"); + timer_ = this->create_wall_timer( + seconds(4), + std::bind(&Exercise3Client::run_next_test, this) + ); + + RCLCPP_INFO(this->get_logger(), "Exercise 3 Client started - Function Combinations"); + } + +private: + int test_case_; + + enum class CombinationType { + SUM, // f + g + DIFFERENCE, // f - g + PRODUCT, // f * g + QUOTIENT, // f / g + COMPOSITION // f(g(x)) + }; + + struct CombinationTest { + int function1_id; + int function2_id; + std::string function1_name; + std::string function2_name; + CombinationType type; + double t; + double h; + int n; + std::string combination_name; + double exact_result; + }; + + // Helper function to get function value + double evaluate_function(int function_id, double x) { + switch(function_id) { + case 1: return std::sin(x); + case 2: return std::cos(x); + case 3: return std::tan(x); + case 4: return std::exp(x); + case 5: return std::log(x); + case 6: return x * x; + case 7: return x * x * x; + case 8: return std::sqrt(x); + case 9: return 1.0 / x; + case 10: return x; + default: return x; + } + } + + // Helper function to get exact derivative + double get_exact_derivative(int function_id, double x) { + switch(function_id) { + case 1: return std::cos(x); // d/dx[sin(x)] = cos(x) + case 2: return -std::sin(x); // d/dx[cos(x)] = -sin(x) + case 3: return 1.0 / (std::cos(x) * std::cos(x)); // d/dx[tan(x)] = sec^2(x) + case 4: return std::exp(x); // d/dx[exp(x)] = exp(x) + case 5: return 1.0 / x; // d/dx[ln(x)] = 1/x + case 6: return 2.0 * x; // d/dx[x^2] = 2x + case 7: return 3.0 * x * x; // d/dx[x^3] = 3x^2 + case 8: return 1.0 / (2.0 * std::sqrt(x)); // d/dx[sqrt(x)] = 1/(2*sqrt(x)) + case 9: return -1.0 / (x * x); // d/dx[1/x] = -1/x^2 + case 10: return 1.0; // d/dx[x] = 1 + default: return 1.0; + } + } + + // Calculate exact derivative for combinations + double calculate_exact_combination_derivative(const CombinationTest& test) { + double t = test.t; + double f_val = evaluate_function(test.function1_id, t); + double g_val = evaluate_function(test.function2_id, t); + double f_prime = get_exact_derivative(test.function1_id, t); + double g_prime = get_exact_derivative(test.function2_id, t); + + switch(test.type) { + case CombinationType::SUM: + return f_prime + g_prime; // (f + g)' = f' + g' + + case CombinationType::DIFFERENCE: + return f_prime - g_prime; // (f - g)' = f' - g' + + case CombinationType::PRODUCT: + return f_prime * g_val + f_val * g_prime; // (f * g)' = f' * g + f * g' + + case CombinationType::QUOTIENT: + if (std::abs(g_val) < 1e-12) return NAN; // Division by zero + return (f_prime * g_val - f_val * g_prime) / (g_val * g_val); // (f/g)' = (f'g - fg')/g^2 + + case CombinationType::COMPOSITION: + // For f(g(x)), we need f'(g(x)) * g'(x) + return get_exact_derivative(test.function1_id, g_val) * g_prime; + + default: + return 0.0; + } + } + + std::vector test_cases_ = { + // SUM RULE: (f + g)' = f' + g' + {1, 2, "sin(x)", "cos(x)", CombinationType::SUM, M_PI/4, 0.01, 15, "sin(x) + cos(x)", 0.0}, + {6, 10, "x^2", "x", CombinationType::SUM, 2.0, 0.01, 15, "x^2 + x", 0.0}, + {4, 5, "exp(x)", "ln(x)", CombinationType::SUM, 1.0, 0.01, 15, "exp(x) + ln(x)", 0.0}, + + // DIFFERENCE RULE: (f - g)' = f' - g' + {1, 2, "sin(x)", "cos(x)", CombinationType::DIFFERENCE, M_PI/6, 0.01, 15, "sin(x) - cos(x)", 0.0}, + {7, 6, "x^3", "x^2", CombinationType::DIFFERENCE, 1.5, 0.01, 15, "x^3 - x^2", 0.0}, + + // PRODUCT RULE: (f * g)' = f' * g + f * g' + {1, 10, "sin(x)", "x", CombinationType::PRODUCT, M_PI/3, 0.01, 15, "sin(x) * x", 0.0}, + {6, 4, "x^2", "exp(x)", CombinationType::PRODUCT, 0.5, 0.01, 15, "x^2 * exp(x)", 0.0}, + {2, 8, "cos(x)", "sqrt(x)", CombinationType::PRODUCT, 1.0, 0.01, 15, "cos(x) * sqrt(x)", 0.0}, + + // QUOTIENT RULE: (f/g)' = (f'g - fg')/g^2 + {1, 10, "sin(x)", "x", CombinationType::QUOTIENT, M_PI/4, 0.01, 15, "sin(x) / x", 0.0}, + {4, 6, "exp(x)", "x^2", CombinationType::QUOTIENT, 1.0, 0.01, 15, "exp(x) / x^2", 0.0}, + {10, 8, "x", "sqrt(x)", CombinationType::QUOTIENT, 4.0, 0.01, 15, "x / sqrt(x)", 0.0}, + + // COMPOSITION (simplified examples where we can calculate) + // Note: These are harder to implement numerically, so we'll use simpler cases + {6, 10, "x^2", "x", CombinationType::COMPOSITION, 2.0, 0.01, 15, "f(g(x)) where f=x^2, g=x", 0.0} + }; + + void run_next_test() { + if (!client_->wait_for_service(seconds(1))) { + RCLCPP_WARN(this->get_logger(), "Service not available"); + return; + } + + if (test_case_ >= test_cases_.size()) { + RCLCPP_INFO(this->get_logger(), "=== Exercise 3 Analysis Complete ==="); + RCLCPP_INFO(this->get_logger(), "All %zu combination test cases completed.", test_cases_.size()); + rclcpp::shutdown(); + return; + } + + auto& test = test_cases_[test_case_]; + + // Calculate exact result + test.exact_result = calculate_exact_combination_derivative(test); + + RCLCPP_INFO(this->get_logger(), + "\n=== Test Case %d ===", test_case_ + 1); + RCLCPP_INFO(this->get_logger(), + "Combination: %s", test.combination_name.c_str()); + RCLCPP_INFO(this->get_logger(), + "At t=%.4f", test.t); + + // Get derivatives of individual functions + auto request1 = std::make_shared(); + request1->h = test.h; + request1->t = test.t; + request1->n = test.n; + request1->function_id = test.function1_id; + + auto request2 = std::make_shared(); + request2->h = test.h; + request2->t = test.t; + request2->n = test.n; + request2->function_id = test.function2_id; + + // Call service for first function + auto future1 = client_->async_send_request(request1, + std::bind(&Exercise3Client::handle_first_response, this, std::placeholders::_1, test, request2)); + } + + void handle_first_response(rclcpp::Client::SharedFuture future1, + CombinationTest test, + std::shared_ptr request2) { + try { + auto response1 = future1.get(); + double f_prime_approx = response1->dfdt; + + // Call service for second function + auto future2 = client_->async_send_request(request2, + std::bind(&Exercise3Client::handle_second_response, this, std::placeholders::_1, test, f_prime_approx)); + + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_logger(), "Failed to get derivative of first function: %s", e.what()); + test_case_++; + } + } + + void handle_second_response(rclcpp::Client::SharedFuture future2, + CombinationTest test, + double f_prime_approx) { + try { + auto response2 = future2.get(); + double g_prime_approx = response2->dfdt; + + RCLCPP_INFO(this->get_logger(), + "f'(%s) ≈ %.6f", test.function1_name.c_str(), f_prime_approx); + RCLCPP_INFO(this->get_logger(), + "g'(%s) ≈ %.6f", test.function2_name.c_str(), g_prime_approx); + + // Calculate combination using derivative rules + double combination_result = 0.0; + std::string rule_description; + + switch(test.type) { + case CombinationType::SUM: + combination_result = f_prime_approx + g_prime_approx; + rule_description = "Sum Rule: (f + g)' = f' + g'"; + break; + + case CombinationType::DIFFERENCE: + combination_result = f_prime_approx - g_prime_approx; + rule_description = "Difference Rule: (f - g)' = f' - g'"; + break; + + case CombinationType::PRODUCT: { + double f_val = evaluate_function(test.function1_id, test.t); + double g_val = evaluate_function(test.function2_id, test.t); + combination_result = f_prime_approx * g_val + f_val * g_prime_approx; + rule_description = "Product Rule: (f * g)' = f' * g + f * g'"; + RCLCPP_INFO(this->get_logger(), + "f(%.4f) = %.6f, g(%.4f) = %.6f", test.t, f_val, test.t, g_val); + break; + } + + case CombinationType::QUOTIENT: { + double f_val = evaluate_function(test.function1_id, test.t); + double g_val = evaluate_function(test.function2_id, test.t); + if (std::abs(g_val) > 1e-12) { + combination_result = (f_prime_approx * g_val - f_val * g_prime_approx) / (g_val * g_val); + rule_description = "Quotient Rule: (f / g)' = (f' * g - f * g') / g^2"; + RCLCPP_INFO(this->get_logger(), + "f(%.4f) = %.6f, g(%.4f) = %.6f", test.t, f_val, test.t, g_val); + } else { + RCLCPP_WARN(this->get_logger(), "Division by zero in quotient rule"); + test_case_++; + return; + } + break; + } + + case CombinationType::COMPOSITION: + // Simplified: assume it's just the composition result + combination_result = f_prime_approx * g_prime_approx; // Simplified chain rule + rule_description = "Chain Rule (simplified): f'(g(x)) * g'(x)"; + break; + } + + RCLCPP_INFO(this->get_logger(), + "Applied %s", rule_description.c_str()); + RCLCPP_INFO(this->get_logger(), + "Combined derivative ≈ %.6f", combination_result); + RCLCPP_INFO(this->get_logger(), + "Exact derivative = %.6f", test.exact_result); + + if (!std::isnan(test.exact_result)) { + double error = std::abs(combination_result - test.exact_result); + double relative_error = (test.exact_result != 0.0) ? + std::abs(error / test.exact_result) * 100.0 : 0.0; + + RCLCPP_INFO(this->get_logger(), + "Absolute error: %.2e", error); + RCLCPP_INFO(this->get_logger(), + "Relative error: %.4f%%", relative_error); + } + + test_case_++; + } + + rclcpp::Client::SharedPtr client_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace homework::two::service_math + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/hw2_service_math/src/service_client.cpp b/src/hw2_service_math/src/service_client.cpp new file mode 100644 index 0000000..19400f2 --- /dev/null +++ b/src/hw2_service_math/src/service_client.cpp @@ -0,0 +1,77 @@ +/* hw2_service_math/service_client.cpp + * Service client that tests the differential quotient calculation service + * + * Reviewed by: + * Changelog: + * [15-09-2025] Wessel T: Implement differential quotient client + */ + +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include "hw2_interface_math/srv/service_vec.hpp" + +using namespace std::chrono; +using hw2_interface_math::srv::ServiceVec; + +namespace hw2::service_math { + +class NodeDifferentialClient : public rclcpp::Node { +public: + NodeDifferentialClient() + : Node("node_differential_service_client") + { + client_ = this->create_client("differential_service"); + timer_ = this->create_wall_timer( + seconds(5), + std::bind(&NodeDifferentialClient::send_request, this) + ); + + RCLCPP_INFO(this->get_logger(), "Differential Client started"); + } + +private: + rclcpp::Client::SharedPtr client_; + rclcpp::TimerBase::SharedPtr timer_; + + void send_request() { + if (!client_->wait_for_service(seconds(1))) { + RCLCPP_WARN(this->get_logger(), "Service not available"); + return; + } + + auto request = std::make_shared(); + request->h = 10.0; + request->t = 2.0; + request->n = 10; + request->function_id = 1; // sin(x) + + auto future = client_->async_send_request(request, + std::bind( + &NodeDifferentialClient::handle_response, this, std::placeholders::_1 + ) + ); + } + + void handle_response(rclcpp::Client::SharedFuture future) { + try { + auto response = future.get(); + RCLCPP_INFO(this->get_logger(), "Received dfdt: %.6lf", response->dfdt); + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Service call failed: %s", e.what()); + } + } +}; + +} // namespace hw2::service_math + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/hw2_service_math/src/service_server.cpp b/src/hw2_service_math/src/service_server.cpp new file mode 100644 index 0000000..c1bdba8 --- /dev/null +++ b/src/hw2_service_math/src/service_server.cpp @@ -0,0 +1,159 @@ +/* hw2_service_math/service_server.cpp + * Service server that calculates numerical approximations for differential quotients + * using the forward difference formula: f(t+h) - f(t) / h + * + * Reviewed by: + * Changelog: + * [04-09-2025] Wessel T: Implement template + * [11-09-2025] Wessel T: + * - (BASE) Working service client + * - Wrap into namespace + * [15-09-2025] Wessel T: Implement differential quotient service + */ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "hw2_interface_math/srv/service_vec.hpp" + +namespace homework::two::service_math { + +using hw2_interface_math::srv::ServiceVec; + +class NodeDifferentialServer : public rclcpp::Node { +public: + NodeDifferentialServer() + : Node("node_differential_service_server") + { + differential_service_server_ = + this->create_service( + "differential_service", + std::bind( + &NodeDifferentialServer::callback_differential_service, + this, + std::placeholders::_1, + std::placeholders::_2 + ) + ); + + RCLCPP_INFO(this->get_logger(), "Differential Service Server started"); + } + + double identifier_to_function(int function_id, double x) { + switch(function_id) { + case 1: return std::sin(x); + case 2: return std::cos(x); + case 3: return std::tan(x); + case 4: return std::exp(x); + case 5: return std::log(x); + case 6: return x * x; + case 7: return x * x * x; + case 8: return std::sqrt(x); + case 9: return 1.0 / x; + case 10: return x; + default: + RCLCPP_WARN(this->get_logger(), "Unknown function id (%d) using f(x) = x", function_id); + return x; + } + } + + double calculate_forward_difference(int function_id, double t, double h) { + double t_as_function = identifier_to_function(function_id, t); + double t_plus_h_as_function = identifier_to_function(function_id, t + h); + return (t_plus_h_as_function - t_as_function) / h; + } + + // Calculate differential quotient using central difference (more accurate) + double calculate_central_difference(int function_id, double t, double h) { + double t_plus_h_as_function = identifier_to_function(function_id, t + h); + double t_minus_h_as_function = identifier_to_function(function_id, t - h); + return (t_plus_h_as_function - t_minus_h_as_function) / (2.0 * h); + } + + void callback_differential_service( + const ServiceVec::Request::SharedPtr request, + ServiceVec::Response::SharedPtr response + ) { + int function_id = request->function_id; + + int n = std::min(request->n, max_iterations_); + double h = request->h; + double t = request->t; + + if (h <= 0) { + RCLCPP_ERROR(this->get_logger(), "expected h >= 0, got: %f", h); + response->dfdt = 0.0; + return; + } + + if (n <= 0) { + RCLCPP_ERROR(this->get_logger(), "expected n >= 0, got: %d", n); + response->dfdt = 0.0; + return; + } + + // Start with initial h and perform bisection + double current_h = h; + double previous_derivative = 0.0; + double current_derivative = 0.0; + + RCLCPP_INFO(this->get_logger(), + "calculating differential for function %d at (t=%f, h=%f, n=%d),", + function_id, t, h, n + ); + + for (int i = 0; i < n; i++) { + // Calculate derivative using forward difference + current_derivative = calculate_forward_difference(function_id, t, current_h); + + RCLCPP_DEBUG(this->get_logger(), + "(i=%d): h=%e, df/dt=%f", i + 1, current_h, current_derivative); + + // Store for convergence checking + if (i > 0) { + double diff = std::abs(current_derivative - previous_derivative); + RCLCPP_DEBUG(this->get_logger(), "convergence diff=%e", diff); + + // early termination if converged + if (diff < 1e-12) { + RCLCPP_INFO(this->get_logger(), "(i=%d) stopping calculation, converged", i + 1); + break; + } + } + + previous_derivative = current_derivative; + current_h = current_h / 2.0; + } + + response->dfdt = current_derivative; + + RCLCPP_INFO(this->get_logger(), + "final result: %f at t=%f", + response->dfdt, t + ); + + // double central_diff = calculate_central_difference(function_id, t, request->h / std::pow(2, n-1)); + // RCLCPP_INFO(this->get_logger(), + // "For comparison, central difference gives: df/dt = %f", central_diff + // ); + } + +private: + rclcpp::Service::SharedPtr differential_service_server_; + + int max_iterations_ {50}; +}; + +} // namespace homework::two::service_math + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +}