Robotics StackExchange | Archived questions

ROS2 Service Call in a Publisher Timer Callback

Running ROS2 Foxy on an RPI4 w/ Ubuntu

I have implemented an SPI service node that provides shared bus access across multiple nodes. This works as expected; I can send a byte array as a request, and the response returns the full duplex recieved data.

However, I am now trying to create another node that publishes data that must be accessed via the SPI service, but I am getting a runtime error:

terminate called after throwing an instance of 'std::runtime_error'
  what():  Node has already been added to an executor.

After researching, I found the various comments online about not being able to call a service from another callback due to deadlocks.

So my question is: what is the 'correct' way to architect this, given that I want to have multipler peripherals on the SPI bus that could be time shared to access different components for different nodes, and I need to grab data via SPI for publishing (or upon recipt if a subscriber).

Thank you, Scott

UPDATE:

Thank you for the response; I have found this link as well with an async call that allows for an automatic callback:

https://github.com/ros2/demos/blob/40e3732791c8c72215a5eb954e9e25be1a5ebb73/demo_nodes_cpp/src/services/add_two_ints_client_async.cpp#L62..L73

Here is the example code:

    // We give the async_send_request() method a callback that will get executed once the response
    // is received.
    // This way we can return immediately from this method and allow other work to be done by the
    // executor in `spin` while waiting for the response.
    using ServiceResponseFuture =
      rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture;
    auto response_received_callback = [this](ServiceResponseFuture future) {
        auto result = future.get();
        RCLCPP_INFO(this->get_logger(), "Result of add_two_ints: %" PRId64, result->sum)
        rclcpp::shutdown();
      };
    auto future_result = client_->async_send_request(request, response_received_callback);
  }

I get the concept that this implementation will therfore permit a callback execution upon receipt of the response, unblocking execution.

However, given my limitied C++ experience, it is not clear how I can wrap my needed function and pass a callback as an argument.

For example, my current implementation is this:

int32_t TMC5130::SpiServiceClient::RegisterRead( uint8_t addr )
{
//Per the datasheet, reading a register requires:
//  1) Each datagram consists of 5 bytes; the first byte contains the target
//      address and a read/write flag as the MS bit.

std::vector<uint8_t> pkt1 = {addr & 0x7F, 0, 0, 0, 0 };
std::vector<uint8_t> pkt2 = {addr & 0x7F, 0, 0, 0, 0 };

auto request = std::make_shared<cpp_rpispi::srv::Spixfer::Request>();

request->tx = BytesToHexList( pkt1 );
request->tx += ",";
request->tx += BytesToHexList( pkt2 );

auto result = this->client_->async_send_request( request );

if( rclcpp::spin_until_future_complete( this->node_, result) == rclcpp::FutureReturnCode::SUCCESS )
{
    //RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "rx: {%s}", result.get()->rx.c_str() );
}
else
{
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "failed");
}

//assume rx is a dual 5 byte datagram
//  [00,11,22,33,44],[55,66,77,88,99]
//  012345678901234567890123456789012
//  0         1         2         3
int32_t data3 = std::stoi( result.get()->rx.substr(21,2), nullptr, 16 );
int32_t data2 = std::stoi( result.get()->rx.substr(24,2), nullptr, 16 );
int32_t data1 = std::stoi( result.get()->rx.substr(27,2), nullptr, 16 );
int32_t data0 = std::stoi( result.get()->rx.substr(30,2), nullptr, 16 );

return (data3<<24 | data2<<16 | data1<<8 | data0 );
}

UPDATE:

Following the information provided, the code is now working using the callback based methods, thank you!

Asked by scott.nortman@gmail.com on 2021-03-20 09:08:39 UTC

Comments

Answers

Nothing prevents to do a service call within a callback managed by the executor (e.g. Timer or subscription callbacks), but a little More care is required for doing it.

The problems with doing a service call within an executor callback are the following:

  • if the service server is located in the same executor as the callback from where you are doing the service request, then the code for the service call must be non blocking. This is because, unless you have multithreaded executor, the service server will not be able to start until the aforementioned callback returns, which is in turn waiting for the service to be completed, causing a deadlock.

  • if the service server is located In another executor, there is no risk of deadlocks, but still you should try to avoid blocking the callback as that will prevent other entities associated with the executor to do their work.

Note that ROS will not detect these problems for you.

As the error says, your problem however is that you are adding your node to two executors. I assume that your node is already added to an executor since it is executing the timer. This means that in the timer callback you can't add it to another. Without looking at the code in your timer callback i can't really say what you are doing wrong, but chances are that you are using a synchronous service call or using a ros API that under the hoods adds the node to an executor, such as spin_until_future_complete.

My suggestion is to look into asynchronous service calls. Those will not cause the problem and will also be better for your system.

For details read the tutorial page that describes both the deadlock problem as well as asynchronous services https://docs.ros.org/en/foxy/Guides/Sync-Vs-Async.html#asynchronous-calls

Asked by alsora on 2021-03-21 04:15:42 UTC

Comments