2

I have a ros2 callback that I'd like to call once a second, which makes several service calls to check the state of another part of the system. Unfortunately, when I include asynchronous service calls (with a small block to wait for a result) the timer callback still finishes, but does not run again. The code is below, and I'm using ROS2 version 'Humble'

Any help would be hugely appreciated

import time
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
from container_interfaces.srv import ReadPLCBool, ReadPLCInt, WritePLCBool, WritePLCInt

from plc_module.utils import error_codes, plc_variables

class MonitorNode(Node):
    
    PLC_POLL_FREQUENCY = 1.0          # How often to poll the PLC to check if a tray has moved into place (in seconds)
                                    
    PLC_SERVICES_TIMEOUT = 10       # How long to wait for the plc services to respond to a request (in seconds) - these are pretty simple services so we shouldn't wait this long unless something has gone wrong
    
    def __init__(self) -> None:
        
        # Call the constructor for the Node class
        super().__init__('monitor')        
        
        # Create various callback groups to ensure that the various services are called in a mutually exclusive manner
        self.plc_reader_callbackgroup = MutuallyExclusiveCallbackGroup()
        
        # Create various clients to interface with the PLC services - if any of these fail to connect the node will not be able to function,
        # so we kill the node immediately if this happens - better than allowing it to run until it tries to access a broken service and crashes
        self._logger.debug("Creating read bool service client")
        self.read_bool_client = self.create_client(ReadPLCBool, 'read_plc_bool', callback_group=self.plc_reader_callbackgroup)
        try:
            self.read_bool_client.wait_for_service(timeout_sec=10)
            self._logger.info("Read bool service client created")
        except:
            self._logger.fatal("'read_plc_bool' service not ready within 10s timeout - is it running?")
            self.destroy_node()
            exit()
        
        # Create a timer to periodically check the state of the PLC and trigger the other modules if necessary
        self.plc_monitor_timer = self.create_timer(self.PLC_POLL_FREQUENCY, self.plc_monitor_callback)
        
       

   def plc_monitor_callback(self) -> bool:
        
        self._logger.info("plc_monitor_callback called")

        request = ReadPLCBool.Request()
        request.data_block = 36
        request.byte_index = 12
        request.bit_index = 1
        
        future = self.read_bool_client.call_async(request)
        
        self._logger.info("Made asynchronous call") 

        while rclpy.ok():
            rclpy.spin_once(self)
            if future.done():
                self._logger.info("Asynchronous call finished")
                try:
                    response = future.result()
                except Exception as e:
                    self.get_logger().error("Service call failed %r" % (e,))
                    break
                else:
                    self._logger.info("Got response: {}".format(response.value))
                    break
                
        self._logger.info("plc_monitor_callback finished")
    
        return 

def main(args=None):
    
    # Create an instance of the node and spin it
    rclpy.init(args=args)
    monitor = MonitorNode()
    
    try:
        rclpy.spin(monitor)
    except KeyboardInterrupt:
        pass

    feed_initialiser.destroy_node()
    rclpy.shutdown()
    
if __name__ == '__main__':
    main()

When I run this code I see all the printed messages up to and including 'plc_monitor_callback finished', but nothing afterwards. The 'read_plc_bool' service is running as expected in another node, and continues to be callable after my monitor node hangs - at the moment it's running in a separate terminal window although the plan is to run these both from a launch file.

I've tried setting up different configurations of callback groups and executors, as well as using synchronous calls to this service, although these attempts don't seem to help.

S. McCarthy
  • 51
  • 2
  • In case it's relevant - the service 'read_plc_bool' doesn't require any other nested service calls - it simply sends a tcp packet and parses the response – S. McCarthy Mar 23 '23 at 11:31

1 Answers1

1

In case anyone runs into this same problem down the line:

I've managed to work around this by removing the block beginning 'while rclpy.ok( ):' and replacing it with the following:

future = self.read_bool_client.call_async(request)    
self._logger.info("Made asynchronous call") 
future.add_done_callback(self.some_new_callback)

Where the new callback takes over the next step in execution - I'd still be interested to hear if there's some way of executing this in a single sequential callback, although this may contravene the general ROS principle of not using synchronous callbacks nested within other callbacks

S. McCarthy
  • 51
  • 2