feat(les2): Turn service into vector calculator

This commit is contained in:
2025-09-11 11:08:00 +02:00
parent 21e3ea4c3d
commit de535b2ee2
4 changed files with 39 additions and 30 deletions

View File

@@ -10,7 +10,7 @@ find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/HardwareStatus.msg"
"srv/ServiceStudent.srv"
"srv/ServiceVec.srv"
)
ament_export_dependencies(rosidl_default_runtime)
# uncomment the following section in order to fill in

View File

@@ -1,7 +1,9 @@
# Student info service message
string sname
# Vector length calculation service message
float64 x
float64 y
float64 z
---
int32 snumber
float64 vec_len
## format <type> <name> <optional_default_value>
##primitive message types

View File

@@ -10,17 +10,17 @@
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "les_interface/srv/service_student.hpp"
#include "les_interface/srv/service_vec.hpp"
using namespace std::chrono;
using les_interface::srv::ServiceStudent;
using les_interface::srv::ServiceVec;
class NodeLes2ServiceClient : public rclcpp::Node {
public:
NodeLes2ServiceClient()
: Node("node_les2_service_client")
{
client_ = this->create_client<ServiceStudent>("student_service_server");
client_ = this->create_client<ServiceVec>("vec_service_server");
timer_ = this->create_wall_timer(
seconds(3),
std::bind(&NodeLes2ServiceClient::send_request, this)
@@ -34,25 +34,28 @@ private:
return;
}
auto request = std::make_shared<ServiceStudent::Request>();
request->sname = "Wessel";
auto request = std::make_shared<ServiceVec::Request>();
request->x = rand() % 100;
request->y = rand() % 100;
request->z = rand() % 100;
auto future = client_->async_send_request(request,
std::bind(&NodeLes2ServiceClient::handle_response, this, std::placeholders::_1)
);
}
void handle_response(rclcpp::Client<ServiceStudent>::SharedFuture future) {
void handle_response(rclcpp::Client<ServiceVec>::SharedFuture future) {
auto response = future.get();
RCLCPP_INFO(this->get_logger(), "Received student id: %ld", response->snumber);
RCLCPP_INFO(this->get_logger(), "Received vec len: %.6lf", response->vec_len);
}
rclcpp::Client<ServiceStudent>::SharedPtr client_;
rclcpp::Client<ServiceVec>::SharedPtr client_;
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
srand(time(NULL));
rclcpp::init(argc, argv);
auto node = std::make_shared<NodeLes2ServiceClient>();
rclcpp::spin(node);

View File

@@ -1,28 +1,29 @@
/* node_template.cpp
* Basic node template for ROS2
/* les2service_server.cpp
* Basic example of a service server node.
*
* Reviewed by: <x>
* Changelog:
* [04-09-2025] Wessel T: Implement template
* [11-09-2025] Wessel T: (BASE) Working service client
*/
#include <cstdlib>
#include "rclcpp/rclcpp.hpp"
#include "les_interface/srv/service_vector.hpp"
#include "les_interface/srv/service_vec.hpp"
using les_interface::srv::ServiceStudent;
using les_interface::srv::ServiceVec;
class NodeLes2ServiceServer : public rclcpp::Node {
public:
NodeLes2ServiceServer()
: Node("node_les2_service_server")
{
student_service_server_ =
this->create_service<ServiceStudent>(
"student_service_server",
vec_service_server_ =
this->create_service<ServiceVec>(
"vec_service_server",
std::bind(
&NodeLes2ServiceServer::callback_student_service,
&NodeLes2ServiceServer::callback_service,
this,
std::placeholders::_1,
std::placeholders::_2
@@ -32,19 +33,22 @@ public:
RCLCPP_INFO(this->get_logger(), "Service Server started");
}
void callback_student_service(
const ServiceStudent::Request::SharedPtr request,
ServiceStudent::Response::SharedPtr response
void callback_service(
const ServiceVec::Request::SharedPtr request,
ServiceVec::Response::SharedPtr response
) {
if (request->sname == "Tilmann") {
response->snumber = 666;
} else {
response->snumber = 42;
}
response->vec_len = sqrt(
pow(request->x, 2) + pow(request->y, 2) + pow(request->z, 2)
);
RCLCPP_INFO(this->get_logger(),
"sqrt(pow(%.6lf), pow(%.6lf), pow(%.6lf))=%.6lf",
request->x, request->y, request->z, response->vec_len
);
}
private:
rclcpp::Service<ServiceStudent>::SharedPtr student_service_server_;
rclcpp::Service<ServiceVec>::SharedPtr vec_service_server_;
};
int main(int argc, char *argv[]) {