ROS2 services using composition.

asked 2021-07-28 04:09:05 -0500

dat-guy gravatar image

Hi,

I am using ROS2 Galactic on Ubuntu 20.04.

I am working with ros2 services with component nodes and not getting the desired behavior.
I tried to show the behavior using the (altered) client component from the demos found here: https://github.com/ros2/demos/tree/ga...

The client code is as follows:

// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "composition/client_component.hpp"

#include <cinttypes>
#include <iostream>
#include <memory>
#include <unistd.h>

#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"

using namespace std::chrono_literals;

namespace composition
{

  enum client_states_t{
    CLIENT_INIT = 0,
    CLIENT_RUN
  };

  enum semaphore_states_t{
    RED = 0,
    GREEN
  };


Client::Client(const rclcpp::NodeOptions & options)
: Node("Client", options)
{
  client_ = create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
  // Note(dhood): The timer period must be greater than the duration of the timer callback.
  // Otherwise, the timer can starve a single-threaded executor.
  // See https://github.com/ros2/rclcpp/issues/392 for updates.
  timer_ = create_wall_timer(2s, std::bind(&Client::on_timer, this));
}

void Client::on_timer()
{
  printf("Before function call\n");
  get_response();
  printf("After function call\n");
}

void Client::get_response()
{
  if(client_state == CLIENT_INIT)
  {
    if (!client_->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(
          this->get_logger(),
          "Interrupted while waiting for the service. Exiting.");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "Service not available after waiting");
      return;
    }

    client_state = CLIENT_RUN;
  }
  else if (client_state == CLIENT_RUN)
  {
    this->semaphore = RED;
    auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
    request->a = 2;
    request->b = 3;

    using ServiceResponseFuture =
      rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture;
    auto response_received_callback = [this](ServiceResponseFuture future) {
        semaphore = GREEN;
        RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", future.get()->sum);
    };
    auto future_result = client_->async_send_request(request, response_received_callback);

    while(this->semaphore == RED)
    {
      usleep(100u);
    }

  }
}

}  // namespace composition

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(composition::Client)

The expected behavior would be:

Before function call  
[Server]: Incoming request: [a: 2, b: 3]  
[Client]: Got result: [5]  
After function call

The actual behavior is that is stuck in the while loop and not getting the response from the server:

Before function call

If I comment the while() loop I get the following output:

Before function call  
After function call  
[INFO] [1627453625.869689016] [Server]: Incoming request: [a: 2, b: 3]  
[INFO] [1627453625.869831475] [Client]: Got result: [5]

Meaning that the response from the server is received after the function ends? Or it receives the response of the server after the next spin?
Is there a way to get the desired behavior while using the method get_response() to get the response from the server?

edit retag flag offensive close merge delete

Comments

printf function print immediately and rclcpp_info take some time to be printed(after the spin), so to measure clearly, print the time in printf to compare clearly

remlog gravatar image remlog  ( 2021-10-27 22:42:07 -0500 )edit