ROS2 Service Client in C++ with Classes with Node Inheritance

[girishkumar.kannan]

Hi The Construct Team,

As I was learning ROS2 Basics in 5 Days Galactic (C++), I found that everything except Service Client had been written with code that had Node Inheritance. It kept bothering me as to why Service Client was not demonstrated with a Node Inheritance code and I tried to come up with my own code, which I got inspired from learning the Action Client code with C++.

I have attached my code below. It works as expected and cleanly exits after calling the service.

I would like to know if this code can be generalized for a “Service Client” replacing the code without the Node Inheritance. [As of 03/10/2022], Service Client example in this course is demonstrated without Node Inheritance or Classes in C++.

Service Client Code [using Node Inheritance]:

#include "rclcpp/logger.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "std_srvs/srv/detail/empty__struct.hpp"
#include "std_srvs/srv/detail/set_bool__struct.hpp"
#include "std_srvs/srv/empty.hpp"
#include "std_srvs/srv/set_bool.hpp"

#include <chrono>
#include <cstdlib>
#include <future>
#include <memory>

using namespace std::chrono_literals;

class ServiceClient : public rclcpp::Node {
private:
  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client_;
  rclcpp::TimerBase::SharedPtr timer_;
  bool service_done_ = false; // inspired from action client c++ code

  void timer_callback() {
    while (!client_->wait_for_service(1s)) {
      if (rclcpp::ok()) {
        RCLCPP_ERROR(
            this->get_logger(),
            "Client interrupted while waiting for service. Terminating...");
        return;
      }
      RCLCPP_INFO(this->get_logger(),
                  "Service Unavailable. Waiting for Service...");
    }

    auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
    // set request variables here, if any
    request->data = true; // comment this line if using Empty() message

    service_done_ = false; // inspired from action client c++ code
    auto result_future = client_->async_send_request(
        request, std::bind(&ServiceClient::response_callback, this,
                           std::placeholders::_1));
  }

  void response_callback(
      rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture future) {
    auto status = future.wait_for(1s);
    if (status == std::future_status::ready) {
      // uncomment below line if using Empty() message
      // RCLCPP_INFO(this->get_logger(), "Result: success");
      // comment below line if using Empty() message
      RCLCPP_INFO(this->get_logger(), "Result: success: %i, message: %s",
                  future.get()->success, future.get()->message.c_str());
      service_done_ = true;
    } else {
      RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
    }
  }

public:
  ServiceClient() : Node("service_client") {
    client_ = this->create_client<std_srvs::srv::SetBool>("move_right");
    timer_ = this->create_wall_timer(
        1s, std::bind(&ServiceClient::timer_callback, this));
    // use below line if you do not want to use
    // "using namespace std::chrono_literals;"
    // timer_ = this->create_wall_timer(
    //     std::chrono::milliseconds(1000),
    //     std::bind(&ServiceClient::timer_callback, this));
  }

  bool is_service_done() const {
    // inspired from action client c++ code
    return this->service_done_;
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);

  // inspired from action client c++ code
  auto service_client = std::make_shared<ServiceClient>();
  while (!service_client->is_service_done()) {
    rclcpp::spin_some(service_client);
  }

  rclcpp::shutdown();
  return 0;
}

// End of Code

Service Server Code [for reference only]:

#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/detail/set_bool__struct.hpp"
#include "std_srvs/srv/set_bool.hpp"

#include <memory>

class ServerNode : public rclcpp::Node {
public:
  ServerNode() : Node("service_move_right") {

    srv_ = create_service<std_srvs::srv::SetBool>(
        "move_right", std::bind(&ServerNode::moving_callback, this,
                                std::placeholders::_1, std::placeholders::_2));
    publisher_ =
        this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
  }

private:
  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr srv_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;

  void moving_callback(
      const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
      const std::shared_ptr<std_srvs::srv::SetBool::Response> response) {
    RCLCPP_INFO(this->get_logger(), "Requested /move_right Service: %d",
                request->data);
    auto message = geometry_msgs::msg::Twist();
    if (request->data == true) {
      message.linear.x = 0.25;
      message.angular.z = -0.25;
      response->message = "Robot Moving";
    } else {
      message.linear.x = 0.0;
      message.angular.z = 0.0;
      response->message = "Robot Stopped";
    }
    publisher_->publish(message);
    response->success = true;
    RCLCPP_INFO(this->get_logger(), "/move_right Service Response: %s",
                response->message.c_str());
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ServerNode>());
  rclcpp::shutdown();
  return 0;
}

// End of Code

Feel free to try it out and let me know if this code is correct and can be used.

Thanks in advance,
Girish

[amalik]

Brilliant .
Your coding is awesome.

[albertoezquerro]

Hello @girishkumar.kannan ,

Great initiative! Initially, it was added like this for simplicity, but I do agree it makes more sense to keep the same structure as the other examples and use classes as well.

I’ll try to analyze and test the code today and if everything is correct I’ll add it to the course.