ros2 humble 如何开启多线程
-
大家好，我在网上找了一个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()