ROS2多线程Python版本_ros2 多线程 python

接着上篇文章

ROS2使用Gazebo控制AGV

，我记录一下ROS2多线程Python版的问题。

提出这个话题的原因是上文中，当需要小车旋转90°时。我最开始的想法是用while循环来实现：

while abs(self.first_yaw-self.yaw)>0.05:
    cmd_vel = Twist()
    cmd_vel.linear.x = 0.0
    cmd_vel.angular.z = angle
    self.cmd_vel_pub.publish(cmd_vel)   
1
2
3
4
5

而不是用if :

if abs(self.first_yaw-self.yaw)>0.05:
cmd_vel = Twist()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = angle
self.cmd_vel_pub.publish(cmd_vel)

显然，使用while会该回调函数 odom_callback 堵塞，导致无法正常获得 self.yaw ，于是小车一直打转 。 如上文，使用if 可以正常运行。

知晓while 出问题的原因，我尝试将整个移动函数与odom_callback分开，另写一个回调函数执行移动函数, 然而这是没用的， while依旧会堵塞主线程

最终通过多线程的方式解决问题 ：

#! /usr/bin/env python3
import rclpy
import math
import numpy as np
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import time
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor

def euler_from_quaternion(quaternion):
    """
    Converts quaternion (w in last place) to euler roll, pitch, yaw
    quaternion = [x, y, z, w]
    """
    x = quaternion.x
    y = quaternion.y
    z = quaternion.z
    w = quaternion.w

    sinr_cosp = 2 * (w * x + y * z)
    cosr_cosp = 1 - 2 * (x * x + y * y)
    roll = np.arctan2(sinr_cosp, cosr_cosp)

    sinp = 2 * (w * y - z * x)
    pitch = np.arcsin(sinp)

    siny_cosp = 2 * (w * z + x * y)
    cosy_cosp = 1 - 2 * (y * y + z * z)
    yaw = np.arctan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw

class CarController(Node):
    def __init__(self):
        super().__init__('car_controller')

        # 目标位置
        self.target_x = 1.0  # 目标 x 坐标
        self.target_y = 1.0  # 目标 y 坐标
        self.current_x = 0.0  # 当前 x 坐标
        self.current_y = 0.0  # 当前 y 坐标
        self.roll=0.0
        self.pitch=0.0
        self.yaw=0.0
        self.group1 = MutuallyExclusiveCallbackGroup()
        self.group2 = MutuallyExclusiveCallbackGroup()

        # 订阅当前位置
        self.odom_sub = self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback,
            10, callback_group=self.group1
        )
        self.odom_sub_1 = self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback_1,
            10, callback_group=self.group2
        )
        # 发布控制指令
        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)

        # 标记当前步骤
        self.current_step = 1

        # 角速度
        self.angular_velocity = math.pi / 2  # 90 度/秒
        self.timer=None
        self.first_yaw =None
        self.timer_count=0

        



    def odom_callback(self, odom_msg):
        self.current_x = odom_msg.pose.pose.position.x
        self.current_y = odom_msg.pose.pose.position.y
        orientation=odom_msg.pose.pose.orientation
        self.roll, self.pitch, self.yaw = euler_from_quaternion(orientation)
        self.get_logger().info('Current Position: x={:.2f}, y={:.2f}, roll={:.2f}, pitch={:.2f}, yaw={:.2f}'.format(self.current_x, self.current_y, self.roll, self.pitch, self.yaw))

        

    def odom_callback_1(self,odom_msg):

        if self.current_step == 1:  # 走完 x 轴
            if abs(self.current_x - self.target_x) < 0.05:
                self.get_logger().info('X direction target position reached!')
                self.current_step = 2  # 进入第二步：旋转
                self.stop_car()
                return

            
            cmd_vel = Twist()
            if self.current_x < self.target_x:
                cmd_vel.linear.x = 0.5  # 正向线速度，可根据需要进行调整
            else:
                cmd_vel.linear.x = -0.5  # 反向线速度，可根据需要进行调整

            self.cmd_vel_pub.publish(cmd_vel)

        elif self.current_step == 2 and abs(self.current_y - self.target_y) > 0.05:  # 根据 y 轴坐标差选择旋转方向
            dy = self.target_y - self.current_y
            if dy > 0:
                angle = math.pi / 2  # 向左旋转 90 度
            else :
                angle = -math.pi / 2  # 向右旋转 90 度
            
            if not self.first_yaw:
                self.first_yaw=self.yaw+angle

            while abs(self.first_yaw-self.yaw)>0.05:
                cmd_vel = Twist()
                cmd_vel.linear.x = 0.0
                cmd_vel.angular.z = angle
                self.cmd_vel_pub.publish(cmd_vel)   


            self.stop_car()
            self.current_step = 3      

        elif self.current_step == 3:  # 走完 y 轴
            if abs(self.current_y - self.target_y) < 0.05:
                self.get_logger().info('Target position reached!')
                self.stop_car()
                return

            cmd_vel = Twist()
            cmd_vel.linear.x = 0.5  # 前进线速度，可根据需要进行调整
            self.cmd_vel_pub.publish(cmd_vel)


    def stop_car(self):
        cmd_vel = Twist()
        self.cmd_vel_pub.publish(cmd_vel)


def main(args=None):
    rclpy.init(args=args)
    car_controller = CarController()
    # executor = MultiThreadedExecutor(num_threads=2)
    executor = SingleThreadedExecutor()
    # Add the node to the executor
    executor.add_node(car_controller)
    try:
        executor.spin()
    finally:
        # Shutdown the executor
        executor.shutdown()
        car_controller.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159

记录一下多线程吧，主要是回调函数的多线程，ROS2中的多线程主要由Executors和Callback Groups组成

1.Executors

ROS2中的执行器分为两类SingleThreadedExecutor 和 MultiThreadedExecutor

SingleThreadedExecutor：同一时刻只能执行一个线程的任务，其他任务需要排队，这个是ROS2默认的执行器，其实当你使用rclpy.spin()时，使用的就是默认的Executor（你可以开两个线程同时执行rclpy.spin()试试 ）

这也是导致为什么while 执行时，其他回调任务也阻塞了。

MultiThreadedExecutor： 允许同时执行多个线程的任务 。当然，直接开启多个线程执行回调函数是没有用的，为什么，就要引出Callback Groups了

2.Callback Groups

ROS2中的执行器分为两类ReentrantCallbackGroup 和 MutuallyExclusiveCallbackGroup。

两者的区别就是ReentrantCallbackGroup允许，同一时刻，一个Group中多个线程同时执行 ？

而MutuallyExclusiveCallbackGroup，同一时刻，一个Group中只能有一个线程执行。而 ROS2中，默认的就是这个回调组，而且不在订阅时声明callback_group=self.group，那么所有

的订阅都在同一个回调组中进行 。因此想要真正使用多线程，需要：

    self.group1 = MutuallyExclusiveCallbackGroup()
    self.group2 = MutuallyExclusiveCallbackGroup()
    self.odom_sub = self.create_subscription(
        Odometry, 'odom', self.odom_callback, 10,  callback_group=self.group1)
    
    executor = MultiThreadedExecutor(num_threads=3)
1
2
3
4
5
6

或者将group声明为ReentrantCallbackGroup，那么只用这样一个组就够了。

想要尝试不同Executor 和 Callback Group的效果，可以执行下面的代码：

自己给参数运行 ，类似： ros2 run xxx_pkg xxx -service_wait_time 5.0 -timer_period 1.0 -callback_group_type reentrant -threads 2

from pickle import TRUE
from std_srvs.srv import SetBool
import rclpy
from rclpy.node import Node
import time

from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
from rclpy.qos import ReliabilityPolicy, QoSProfile

import argparse


class DummyServer(Node):

    def __init__(self, args, callback_group_type="reentrant"):

        self.timer_flag = True

        super().__init__('service_start_turn')

        # More info here: https://docs.python.org/3/library/argparse.html
        parser = argparse.ArgumentParser(
            description='Dummy Server to Learn about Callback Groups and Threads')

        # Add an argument for setting the service waiting time
        parser.add_argument('-service_wait_time',
                            type=float,
                            help='Time the service will be waiting',
                            required=True)

        # Add an argument for setting por of the timer callback
        parser.add_argument('-timer_period',
                            type=float,
                            nargs=1,
                            metavar='TIMEOUT',
                            default=1.0,
                            help="Time period of the Callback for the timer")

        # Add an argument for setting the callback group type
        parser.add_argument('-callback_group_type',
                            type=str,
                            default="reentrant",
                            help="Type of Callback Groups REENTRANT of EXCLUSIVE")

        # Add an argument for setting the number of threads
        parser.add_argument('-threads',
                            type=int,
                            default=1,
                            help="Number of threads to use in the executor")

        self.args = parser.parse_args(args[1:])

        parser.print_help()

        print("## DEFAULT Node Callback Group=" +
            str(self.default_callback_group))

        self.get_logger().warning("Setting "+self.args.callback_group_type+" Groups")
        if self.args.callback_group_type == "reentrant":
            # If you set the group reentrant, any Callback inside will be executed in parallel
            # If there are enough threads
            self.group1 = ReentrantCallbackGroup()
            self.get_logger().warning("ReentrantCallbackGroup Set")
            # Both the service and the timer use the same callback group
            self.srv = self.create_service(
                SetBool, '/dummy_server_srv', self.SetBool_callback, callback_group=self.group1)
            self.timer = self.create_timer(
                self.args.timer_period[0], self.timer_callback, callback_group=self.group1)

        elif self.args.callback_group_type == "exclusive":
            self.group1 = MutuallyExclusiveCallbackGroup()
            self.group2 = MutuallyExclusiveCallbackGroup()
            self.get_logger().warning("MutuallyExclusiveCallbackGroup Set")
            # Set one group for the service and another one for the timer
            self.srv = self.create_service(
                SetBool, '/dummy_server_srv', self.SetBool_callback, callback_group=self.group1)
            self.timer = self.create_timer(
                self.args.timer_period[0], self.timer_callback, callback_group=self.group2)

        else:
            # You do not set groups. Therefore, they will get the default group for the Node
            self.get_logger().error("NO GROUPS SET Set")
            self.srv = self.create_service(
                SetBool, '/dummy_server_srv', self.SetBool_callback)
            self.timer = self.create_timer(
                self.args.timer_period[0], self.timer_callback)

    def get_threads(self):
        return self.args.threads

    def SetBool_callback(self, request, response):
        self.get_logger().warning("Processing Server Message...")
        self.wait_for_sec(self.args.service_wait_time)
        self.get_logger().warning("Processing Server Message...DONE")
        response.message = 'TURNING'
        # return the response parameters
        return response

    def wait_for_sec(self, wait_sec, delta=1.0):
        i = 0
        while i < wait_sec:
            self.get_logger().info("..."+str(i)+"[WAITING...]")
            time.sleep(delta)
            i += delta

    def timer_callback(self):
        self.print_dummy_msgs()

    def print_dummy_msgs(self):
        if self.timer_flag:
            self.get_logger().info("TICK")
            self.timer_flag = False
        else:
            self.get_logger().info("TACK")
            self.timer_flag = True


def main(args=None):

    rclpy.init(args=args)
    print("args==="+str(args))
    args_without_ros = rclpy.utilities.remove_ros_args(args)
    print("clean ROS args==="+str(args_without_ros))
    start_stop_service_node = DummyServer(args_without_ros)

    num_threads = start_stop_service_node.get_threads()
    start_stop_service_node.get_logger().info(
        'DummyServer Started with threads='+str(num_threads))

    executor = MultiThreadedExecutor(num_threads=num_threads)
    executor.add_node(start_stop_service_node)

    try:
        executor.spin()
    finally:
        executor.shutdown()
        start_stop_service_node.destroy_node()

    # shutdown the ROS communication
    rclpy.shutdown()


if __name__ == '__main__':
    main()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145