ROS2(Python)Timer回调只在调用spin_once()时调用一次

b5lpy0ml  于 2023-03-31  发布在  Python
关注(0)|答案(1)|浏览(401)

我有一个ros 2回调,我想每秒调用一次,它会进行几次服务调用来检查系统另一部分的状态。不幸的是,当我包括异步服务调用(用一个小块来等待结果)时,计时器回调仍然完成,但不会再次运行。代码如下,我使用的是ROS 2版本'Humble'
任何帮助都将不胜感激

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

当我运行这段代码时,我看到所有打印的消息,直到并包括'plc_monitor_callback finished',但之后什么也没有。'read_plc_bool'服务在另一个节点中按预期运行,并且在我的监视器节点挂起后继续可调用-目前它在一个单独的终端窗口中运行,尽管计划是从启动文件运行这两个窗口。
我尝试过设置回调组和执行器的不同配置,以及使用对此服务的同步调用,尽管这些尝试似乎没有帮助。

uinbv5nw

uinbv5nw1#

万一有人遇到同样的问题,
我已经设法解决了这个问题,删除了以'while rclpy.ok():'开头的代码块,并将其替换为以下代码:

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

新回调在哪里接管执行的下一步-我仍然很想知道是否有某种方法可以在单个顺序回调中执行它,尽管这可能违反不使用嵌套在其他回调中的同步回调的一般ROS原则

相关问题