inholland/ros2-homework
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: All other homework assignments

Browse Source
This commit is contained in:
Wessel T
2025-11-13 18:49:09 +01:00
parent 09e5abbcb7
commit 89f25e47f1
9 changed files with 853 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/hw2_interface_math/CMakeLists.txt Normal file
View File

@@ -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(<dependency> REQUIRED)
ament_package()

21
src/hw2_interface_math/package.xml Normal file
View File

@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>hw2_interface_math</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="wessel@todo.todo">wessel</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

19
src/hw2_interface_math/srv/ServiceVec.srv Normal file
View File

@@ -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 <type> <name> <optional_default_value>
##primitive message types
# bool
# byte
# char
# float32, float64
# int8, uint8
# int16, uint16
# int32, uint32
# int64, uint64
# string

44
src/hw2_service_math/CMakeLists.txt Normal file
View File

@@ -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()

21
src/hw2_service_math/package.xml Normal file
View File

@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>hw2_service_math</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="wessel@go2it.eu">wessel</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>hw2_interface_math</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

181
src/hw2_service_math/src/exercise2_client.cpp Normal file
View File

@@ -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: <x>
* Changelog:
* [15-09-2025] Wessel T: Implement exercise 2 client
*/
#include <chrono>
#include <cstdlib>
#include <cmath>
#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<ServiceVec>("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<TestCase> 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<ServiceVec::Request>();
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<ServiceVec>::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<homework::two::service_math::Exercise2Client>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

313
src/hw2_service_math/src/exercise3_client.cpp Normal file
View File

@@ -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: <x>
* Changelog:
* [15-09-2025] Wessel T: Implement exercise 3 client
*/
#include <chrono>
#include <cstdlib>
#include <cmath>
#include <string>
#include <vector>
#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<ServiceVec>("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<CombinationTest> 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<ServiceVec::Request>();
request1->h = test.h;
request1->t = test.t;
request1->n = test.n;
request1->function_id = test.function1_id;
auto request2 = std::make_shared<ServiceVec::Request>();
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<ServiceVec>::SharedFuture future1,
CombinationTest test,
std::shared_ptr<ServiceVec::Request> 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<ServiceVec>::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<ServiceVec>::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<homework::two::service_math::Exercise3Client>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

77
src/hw2_service_math/src/service_client.cpp Normal file
View File

@@ -0,0 +1,77 @@
/* hw2_service_math/service_client.cpp
* Service client that tests the differential quotient calculation service
*
* Reviewed by: <x>
* Changelog:
* [15-09-2025] Wessel T: Implement differential quotient client
*/
#include <chrono>
#include <cstdlib>
#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<ServiceVec>("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<ServiceVec>::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<ServiceVec::Request>();
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<ServiceVec>::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<hw2::service_math::NodeDifferentialClient>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

159
src/hw2_service_math/src/service_server.cpp Normal file
View File

@@ -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: <x>
* 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 <cstdlib>
#include <cmath>
#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<ServiceVec>(
"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<ServiceVec>::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<homework::two::service_math::NodeDifferentialServer>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}