https://github.com/ijnek/nested_services_rclcpp_demo
Demo of calling services from within callbacks in rclcpp (ROS 2)
https://github.com/ijnek/nested_services_rclcpp_demo
rclcpp ros2 ros2-humble ros2-iron ros2-rolling
Last synced: 6 months ago
JSON representation
Demo of calling services from within callbacks in rclcpp (ROS 2)
- Host: GitHub
- URL: https://github.com/ijnek/nested_services_rclcpp_demo
- Owner: ijnek
- Created: 2024-02-28T23:12:32.000Z (about 2 years ago)
- Default Branch: main
- Last Pushed: 2024-02-28T23:45:07.000Z (about 2 years ago)
- Last Synced: 2024-02-29T00:37:53.167Z (about 2 years ago)
- Topics: rclcpp, ros2, ros2-humble, ros2-iron, ros2-rolling
- Language: C++
- Homepage:
- Size: 2.93 KB
- Stars: 0
- Watchers: 1
- Forks: 0
- Open Issues: 0
-
Metadata Files:
- Readme: README.md
Awesome Lists containing this project
README
# Demo of calling services from within callbacks
A major pain point of ROS 2 is not being able to call services from within any sort of callback.
This is caused by the executor being occupied by the callback, and not free to process service responses, causing a deadlock.
Many forum posts suggest using multiple callback groups to solve this problem, but this involves creating
multiple threads, which is not always desirable. It also means your node will require a MultiThreadedExecutor.
This solution provides a way to use a SingleThreadedExecutor, and still be able to call services from within callbacks. Only one executor thread is used, and it is not blocked by the callback.
It utilizes asynchronous service calls and **deferred service responses**, a feature available since ROS 2 Humble.
In this demo, NodeA and NodeB are created, and added to a SingleThreadedExecutor, and spun. Nothing special.
```cpp
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor executor;
auto node1 = std::make_shared();
auto node2 = std::make_shared();
executor.add_node(node1);
executor.add_node(node2);
executor.spin();
rclcpp::shutdown();
return 0;
}
```
Node1 provides a service of type `std_srvs/srv/Trigger` on `node1/trigger`.
It also creates a client to call a service of type `std_srvs/srv/Trigger` on `node2/trigger`.
```cpp
class Node1 : public rclcpp::Node
{
public:
Node1() : rclcpp::Node("node1")
{
service_ = create_service(
"node1/trigger", std::bind(&Node1::srv_cb, this, _1, _2, _3));
client_ = create_client("node2/trigger");
}
private:
void srv_cb(std::shared_ptr> service,
const std::shared_ptr request_header,
const std::shared_ptr request)
{
...
}
rclcpp::Service::SharedPtr service_;
rclcpp::Client::SharedPtr client_;
};
```
NodeB provides a service of type `std_srvs/srv/Trigger` on `node2/trigger`, which simply responds with a message saying ``"Hello!"``.
```cpp
class Node2 : public rclcpp::Node
{
public:
Node2() : rclcpp::Node("node2")
{
service_ = create_service(
"node2/trigger", std::bind(&Node2::srv_cb, this, _1, _2));
}
private:
void srv_cb(const std::shared_ptr request,
std::shared_ptr response)
{
(void)request;
response->message = "Hello!";
}
rclcpp::Service::SharedPtr service_;
};
```
Now, going back to Node1's srv_cb function, this function's desired behavior is to call the ``node2/trigger`` service, prepend the response's message with ``"Node 2 said: "``, and send that back.
To do this, we create an asynchronous callback function, and call the second service with ``async_send_request`` function. This allows the executor to finish srv_cb and free up without returning a response. The executor can then listen to the service response from Node2. When the service response is received, the ``async_cb`` lambda is called, which sends a deferred response back to the original service call.
Note the callback signature is different to the ones in the basic service examples. This is because we are using a deferred response.
```cpp
void srv_cb(std::shared_ptr> service,
const std::shared_ptr request_header,
const std::shared_ptr request)
{
auto async_cb = [service, request_header, request](rclcpp::Client::SharedFuture future) {
(void)request;
std_srvs::srv::Trigger::Response response;
response.message = "Node 2 said: '" + future.get()->message + "'";
service->send_response(*request_header, response);
};
auto request_inner = std::make_shared();
client_->async_send_request(request_inner, async_cb);
}
```
To test this, run the program:
```
ros2 run nested_services_rclcpp_demo nested_service
```
and call the service from the command line:
```bash
ros2 service call /node1/trigger std_srvs/srv/Trigger
```
You should expect a response:
```bash
ros2 service call /node1/trigger std_srvs/srv/Trigger
requester: making request: std_srvs.srv.Trigger_Request()
response:
std_srvs.srv.Trigger_Response(success=False, message="Node 2 said: 'Hello!'")
```