I'm a beginner in ROS, and I tried to create a client that requests a service to add 2 numbers generated randomly every 2 seconds. The service receives the request and sends back the result, but it seems that the client is not able to receive it. I'm not sure where the issue lies, so I would greatly appreciate your help in identifying the problem (or problems). Also my first question here so sorry in advice if I did something wrong!
This is the client code:
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
#include <chrono>
#include <cstdlib>
#include <memory>
#include<stdlib.h> //para rand()
#include<time.h> //para rand()
using namespace std::chrono_literals;
using namespace rclcpp;
class MyClient: public Node
{
public :
//Constructor
MyClient() : Node("add_two_ints_client"), counter_(0)
{
RCLCPP_INFO(this->get_logger(), "Hey add_two_ints_client"); // hacemos un print en el nodo
client = this->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints"); // creo cliente
//while (!client->wait_for_service(1s))
if (!client->wait_for_service(3s))
{
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
rclcpp::shutdown();
}
request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>(); // inicializo la request (opcional)
timer_ = create_wall_timer(std::chrono::seconds(2), bind(&MyClient::timerCallback, this)); // usamos bind porque estamos en una clase, this se refiere al objeto MyNode
}
//Destructor
~MyClient()
{
RCLCPP_INFO(this->get_logger(), "Bye add_two_ints_client"); // hacemos un print en el nodo
}
private:
void timerCallback()
{
counter_++;
request->a = (long long int)(rand()%20);
request->b = (long long int)(rand()%10);
RCLCPP_INFO(this->get_logger(), "Request %d -> Sumar: %ld + %ld", counter_, request->a, request->b);
auto result = client->async_send_request(request); // mando la request
timer_->cancel(); // paro el timer de la callback
// Esperar hasta que la tarea futura se complete con un timeout de 5s
auto wait_result = result.wait_for(std::chrono::seconds(5));
if (wait_result == std::future_status::ready) // obtengo respuesta del servicio
{
RCLCPP_INFO(this->get_logger(), "Resultado: %ld\n\n", result.get()->sum);
timer_->reset(); // reanudo el timer
}
else if (wait_result == std::future_status::timeout) // no obtengo respuesta y salta el time out
{
RCLCPP_INFO(this->get_logger(), "Tiempo de espera agotado. Error. Exiting.");
rclcpp::shutdown();
}
}
TimerBase::SharedPtr timer_;
int counter_;
rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client;
std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request;
};
int main(int argc, char **argv)
{
srand(time(NULL));
rclcpp::init(argc, argv);
auto node = std::make_shared<MyClient>(); //creo nodo
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
This is the service code:
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
#include <memory>
void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response)
{
response->sum = request->a + request->b;
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",
request->a, request->b);
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
} // suma dos valores e imprime la info por la consola. Es la funcion vinculada al servicio
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server"); //creo nodo
rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service =
node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add); //creo servicio
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints."); //hago print en la consola
rclcpp::spin(node); // lanzo el nodo
rclcpp::shutdown();
}
(I'm building with colcom).
This is an example of the results I got on the terminal:
Client:
[INFO] [1690571647.514563437] [add_two_ints_client]: Hey add_two_ints_client [INFO] [1690571649.515272716] [add_two_ints_client]: Request 1 -> Sumar: 5 + 4 [INFO] [1690571654.515567553] [add_two_ints_client]: Tiempo de espera agotado. Error. Exiting. [INFO] [1690571654.517240678] [add_two_ints_client]: Bye add_two_ints_client
Service:
[INFO] [1690571649.515536003] [rclcpp]: Incoming request a: 5 b: 4 [INFO] [1690571649.515564808] [rclcpp]: sending back response: [9]