ros2 humble 如何开启多线程

[13838525575]

大家好，我在网上找了一个ros2的学习例程，https://blog.csdn.net/aibingjin/article/details/124005325?ydreferer=aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2FpYmluZ2ppbi9jYXRlZ29yeV8xMTcyNTExOS5odG1s
上述例程是有一个msg控制小车前后左右运动，一个service控制背景切换（与想问的问题无关），一个action控制小车左转右转直行等。作者在最下面的main中定义了一个 executor = MultiThreadedExecutor()，实现这几个接口的同时运行。（个人理解，不知道对不对。）但是作者用的ros2是foxy版本，所以他使用下面的代码可以正常运行。但是我用humble却跑不起来，下面的代码可以正常接收到msg命令，也可以接收到action命令，但是小车不动。我理解应该是多线程的问题。因为我把多线程去掉，小车可以正常执行msg收到的命令。
请问有没有大神可以帮忙看一下是怎么回事？

duckiebot_node.py

#!/usr/bin/env python3
 
import rclpy
from rclpy.node import Node
 
import gym
from pyglet.window import key
from gym_duckietown.envs import DuckietownEnv
 
import cv2
import numpy as np
import time  #动作是一段时间内完成的，引入time库使用时间相关函数
 
from sensor_msgs.msg import Image  #发布图像使用Image消息类型
from duckietown_interface.msg import Twist2DStamped #控制消息类型
from duckietown_interface.srv import ChangeMap
from cv_bridge import CvBridge #opencv和ros图像数据转换工具
 
#引入ActionServer库
from rclpy.action import ActionServer
#引入新建的动作接口
from duckietown_interface.action import Crossing
#引入多线程执行器，将节点支持多线程，否则执行动作时会阻塞主线程
from rclpy.executors import MultiThreadedExecutor
 
class DuckiebotNode(Node): 
    def __init__(self, name):
        super().__init__(name)
        self.env = DuckietownEnv(seed=1,map_name="udem1",draw_curve=False,draw_bbox=False,domain_rand=False,frame_skip=1,distortion=False,camera_rand=False,dynamics_rand=False)
        self.env.reset()
        #定义图像发布接口
        self.pub_img = self.create_publisher(Image,"duckiebot_node/image",10)
        #修改定时器频率，仿真器默认是30Hz，保持一致，方便计算
        self.timer = self.create_timer(1/30, self.timer_callback)
        #创建图像转换工具
        self.bridge = CvBridge()
        #定义全局动作变量，默认线速度和角速度都是0，车辆停止
        self.action = np.array([0.0,0.0])
        #订阅控制指令话题
        self.sub_action = self.create_subscription(Twist2DStamped, "control_node/action", self.cb_action, 10)
        self.cm_srv = self.create_service(ChangeMap, 'change_map_name', self.cb_change_map)
        
        #定义变量，用来标识动作是否在执行，执行过程中禁用其他车辆控制指令
        self.do_action = False
        #创建动作服务
        self.crossing_server = ActionServer(self,Crossing,'crossing_acton',self.execute_callback)
    
    def timer_callback(self):
        #这里不再生成随机动作指令，直接使用全局动作变量
        obs, reward, done, info = self.env.step(self.action)
        #发布图像数据，obs是rgb编码，转化时指定编码，解码时就有据可查
        self.pub_img.publish(self.bridge.cv2_to_imgmsg(obs, 'rgb8'))
        if done:
            self.env.reset()
    #修改键盘控制车辆指令回调函数，执行动作期间，禁用车辆控制指令
    def cb_action(self, msg):
        if self.do_action:
            return
        v = msg.twist2d.v       #线速度
        omega = msg.twist2d.omega  #角速度
        self.action[0] = v
        self.action[1] = omega
    
    def cb_change_map(self, request, response):
        map_name = request.map_name
        try:
            self.env.close()
            self.env = DuckietownEnv(seed=1,map_name=map_name,draw_curve=False,draw_bbox=False,domain_rand=False,frame_skip=1,distortion=False,camera_rand=False,dynamics_rand=False)
            self.env.reset()
            response.result = True
        except Exception:
            response.result = False
        return response
    
     #动作服务回调函数，所有动作指令在这里实现
    def execute_callback(self, goal_handle):
        #开始执行动作，利用do_action变量禁用键盘控制指令
        self.do_action = True
        #从动作请求中获取速度speed和方向direction参数
        speed = goal_handle.request.speed
        direction = goal_handle.request.direction    
        #计算计算实际执行过程中的相关参数：实际线速度、角速度、执行时间、前置执行时间
        real_speed, omega, need_time, prev_time = self.get_action_args(speed, direction)
        #创建反馈主题数据
        feedback_msg = Crossing.Feedback()
        #反馈主题反馈动作完成度百分比的倒计数
        feedback_msg.countdown = 100.0
        #记录当前时间
        current_t = time.time()
        #记录开始时间
        start_t = current_t
        #执行时间结束前，循环执行
        while current_t-start_t<need_time:
            #在前置时间内，需要直行走过0.07米
            if current_t-start_t<prev_time:
                self.action = np.array([real_speed,0.0])
            else: #剩余的时间内，按计算出的线速度和角速度行驶
                self.action = np.array([real_speed, omega])
            #计算剩余时间占总执行时间的百分比
            feedback_msg.countdown = (1-(current_t-start_t)/need_time)*100
            #通过反馈主题发送给客户端
            goal_handle.publish_feedback(feedback_msg)
            #线程休眠0.1秒，继续循环
            time.sleep(0.1)
            #更新当前时间
            current_t = time.time()
        #动作执行完成后，设置目标完成标识
        goal_handle.succeed()
        #在结果服务中，设置结果响应数据
        result = Crossing.Result()
        result.result = True
        #清除动作执行期间的数据
        self.action = np.array([0.0,0.0])
        #启用键盘控制指令
        self.do_action = False
        return result
    
    #根据指定的动作参数（速度：speed，方向：direction）计算实际执行过程中的相关参数
    def get_action_args(self, speed, direction):
        #计算真实需要输入的线速度（目前只是根据实际现象得知：实际速度=给定速度*0.7，为什么是0.7需要仔细读一下仿真源码）
        real_speed =  speed/0.7
        #预设看到红线时开始执行动作，
        #小车摄像头前倾20°左右，根据小车高度和摄像头视角等参数，
        #计算得到红线在摄像头观察图像下边缘时，车辆距离红线约等于7cm
        #这段距离需要车辆先直行一段时间，这段时间下文定义为前置时间（prev_time）
        dist = 0.07
        #动作期间要转过得角度
        angular = 0
        if direction=='left':    #左转弧线距离0.66米，转角90°
            dist += 0.66
            angular = np.pi/2
        elif direction=='right': #右转弧线距离0.26米，转角-90°
            dist += 0.26
            angular = -np.pi/2
        else:                    #直行距离0.585米，转角为0
            dist += 0.585
        need_time = dist/speed   #根据距离计算动作执行时间
        prev_time = 0.07/speed   #计算前置时间，直行通过0.07米的时间
        #计算转弯时角速度，4/9的关系是根据实践大致计算出来的，具体原理需要研读仿真器源码
        omega = angular/(need_time-prev_time)/4*9
        return real_speed, omega, need_time, prev_time
        
 
def main(args=None):
    rclpy.init(args=args)
    node = DuckiebotNode(name="duckiebot_node")
    executor = MultiThreadedExecutor()
    rclpy.spin(node=node,executor=executor)
    rclpy.shutdown()