Ask Your Question
0

ROS2 Service Call in a Publisher Timer Callback

asked 2021-03-20 09:08:39 -0600

scott.nortman@gmail.com gravatar image

updated 2021-03-23 18:43:48 -0600

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/40...

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 ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2021-03-21 04:15:42 -0600

alsora gravatar image

updated 2021-03-21 04:23:41 -0600

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/S...

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

4 followers

Stats

Asked: 2021-03-20 09:08:39 -0600

Seen: 467 times

Last updated: Mar 23