ROS2 Managed node: How to make a service call in 'on_configure'?
Hi all,
I have a Python node that queries information on startup via a service call. Recently, I tried to convert the node to a lifecycle node where the queries are performed in on_configure()
(I also tried on_activate()
) but execution gets stuck when trying to receive the service response. Probably this is due to the node not being in an active event loop in that state.
Here's a minimal example showing the problem (you need the lifecycle-py
package to be installed):
from typing import Optional
import rclpy
from rclpy.lifecycle import Node
from rclpy.lifecycle import Publisher
from rclpy.lifecycle import State
from rclpy.lifecycle import TransitionCallbackReturn
from rclpy.timer import Timer
import std_msgs.msg
from example_interfaces.srv import AddTwoInts
class ManagedNode(Node):
def __init__(self, node_name, **kwargs):
self._count: int = 0
self._pub: Optional[Publisher] = None
self._timer: Optional[Timer] = None
self._cli = None
super().__init__(node_name, **kwargs)
def publish(self):
msg = std_msgs.msg.String()
msg.data = "Lifecycle HelloWorld #" + str(self._count);
self._count += 1
if self._pub is None or not self._pub.is_activated:
self.get_logger().info('Lifecycle publisher is currently inactive. Messages are not published.');
else:
self.get_logger().info(f'Lifecycle publisher is active. Publishing: [{msg.data}]');
if self._pub is not None:
self._pub.publish(msg)
def send_request(self, a, b):
req = AddTwoInts.Request()
req.a = a
req.b = b
future = self._cli.call_async(req)
rclpy.spin_until_future_complete(self, future)
print(f'result: {future}')
def on_configure(self, state: State) -> TransitionCallbackReturn:
self._pub = self.create_lifecycle_publisher(std_msgs.msg.String, "lifecycle_chatter", 10);
self._timer = self.create_timer(1.0, self.publish)
self._cli = self.create_client(AddTwoInts, 'add_two_ints')
while not self._cli.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...')
self.send_request(2, 3)
self.get_logger().info("on_configure() is called.")
return TransitionCallbackReturn.SUCCESS
def on_cleanup(self, state: State) -> TransitionCallbackReturn:
self.destroy_timer(self._timer)
self.destroy_publisher(self._pub)
self.get_logger().info('on_cleanup() is called.')
return TransitionCallbackReturn.SUCCESS
def on_shutdown(self, state: State) -> TransitionCallbackReturn:
self.destroy_timer(self._timer)
self.destroy_publisher(self._pub)
self.get_logger().info('on_shutdown() is called.')
return TransitionCallbackReturn.SUCCESS
def main():
rclpy.init()
executor = rclpy.executors.SingleThreadedExecutor()
node = ManagedNode('managed_node')
executor.add_node(node)
try:
executor.spin()
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
node.destroy_node()
if __name__ == '__main__':
main()
Steps to reproduce:
- Start the node:
python3 managed_node.py
- Start the service:
ros2 run examples_rclcpp_minimal_service service_main
- Perform transition on node:
ros2 lifecycle set /managed_node configure
- The node is stuck. If
send_request()
is commented out everything is working fine
Does anyone know if/how it's possible to make service calls in the early stages of a managed node?
Thanks! Thomas
Asked by yurumi on 2023-04-04 06:49:17 UTC
Answers
As far as I understand the problem, is that the executor is spinning your node, and when the node wants to process a service, it halts spinning to let the node complete the service. The service then calls another service, which requires some other node to spin, which it is being hindered from since the executor is explicitly waiting for your service to finish before spinning. This is a deadlock, and is the primary reason why async calls to services are preferred in ROS 2.
Have a look at this answer, I think you can get around it using different callback groups. https://answers.ros.org/question/413482/ros2-asyncio-await-with-timeout-a-service-call-in-a-callback/
But whenever I encounter this situation, I think it calls for a redesign.
Asked by Per Edwardsson on 2023-04-04 08:06:13 UTC
Comments