ROS2代码阅读笔记5-ROS2小车巡逻功能

#for patrol
#math
import math
from math import radians, copysign, sqrt, pow
from math import pi
import numpy as np
#rclpy
import rclpy
from rclpy.node import Node
#tf
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from tf2_ros import LookupException, ConnectivityException, ExtrapolationException
#msg
from geometry_msgs.msg import Twist, Point,Quaternion
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Bool
#others
import PyKDL
from time import sleep

print("import finish")

RAD2DEG = 180 / math.pi

class YahboomCarPatrol(Node):
    def __init__(self,name):
        super().__init__(name)
        self.moving = True
        self.Joy_active = False
        self.command_src = "finish"
        self.warning = 1
        self.SetLoop = False
        self.Linear = 0.5
        self.Angular = 1.0
        self.Length = 1.0 #1.0
        self.Angle = 360.0
        self.LineScaling = 1.1
        self.RotationScaling = 1.0
        self.LineTolerance = 0.1
        self.RotationTolerance = 0.3
        #self.ResponseDist = 0.6
        #self.LaserAngle = 20
        self.warning = 1
        #self.Command = "LengthTest"
        #self.Switch = False
        self.position = Point()
        self.x_start = self.position.x
        self.y_start = self.position.y
        self.error = 0.0
        self.distance = 0.0 
        self.last_angle = 0.0 
        self.delta_angle = 0.0
        self.turn_angle = 0.0
        #create publisher
        self.pub_cmdVel = self.create_publisher(Twist,"/cmd_vel",5)
        #create subscriber
        self.sub_scan = self.create_subscription(LaserScan,"/scan",self.LaserScanCallback,1)
        self.sub_joy = self.create_subscription(Bool,"/JoyState",self.JoyStateCallback,1)
        #create TF
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer,self)
        #declare param
        self.declare_parameter('odom_frame',"odom")
        self.odom_frame = self.get_parameter('odom_frame').get_parameter_value().string_value
        self.declare_parameter('base_frame',"base_footprint")
        self.base_frame = self.get_parameter('base_frame').get_parameter_value().string_value
        self.declare_parameter("circle_adjust",2.0)
        self.circle_adjust = self.get_parameter("circle_adjust").get_parameter_value().double_value
        self.declare_parameter('Switch',False)
        self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
        self.declare_parameter('Command',"Square")
        self.Command = self.get_parameter('Command').get_parameter_value().string_value
        self.declare_parameter('Set_loop',False)
        self.Set_loop = self.get_parameter('Set_loop').get_parameter_value().bool_value
        self.declare_parameter('ResponseDist',0.6)
        self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
        self.declare_parameter('LaserAngle',20.0)
        self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
        self.declare_parameter('Linear',0.5)
        self.Linear = self.get_parameter('Linear').get_parameter_value().double_value
        self.declare_parameter('Angular',1.0)
        self.Angular = self.get_parameter('Angular').get_parameter_value().double_value
        self.declare_parameter('Length',1.0)
        self.Length = self.get_parameter('Length').get_parameter_value().double_value
        self.declare_parameter('RotationTolerance',0.3)
        self.RotationTolerance = self.get_parameter('RotationTolerance').get_parameter_value().double_value
        self.declare_parameter('RotationScaling',1.0)
        self.RotationScaling = self.get_parameter('RotationScaling').get_parameter_value().double_value
        
        #create a timer
        self.timer = self.create_timer(0.01,self.on_timer)
        self.index = 0

这段代码定义了一个名为 YahboomCarPatrol 的 ROS 2 节点类，该类继承自 Node 类。此节点主要用于控制 Yahboom 小车进行巡逻任务，如沿着特定路径移动、检测障碍物并做出响应等。

下面是这段代码的主要组成部分和功能说明：

1. 导入必要的模块:

   • 导入了数学相关的函数和常量（例如 math 和 numpy）。
   • 导入了 ROS 2 相关的模块，如 rclpy 和 Node。
   • 导入了 TF2（Transforms）相关的类，用于坐标变换。
   • 导入了消息类型，如 Twist 和 LaserScan。
   • 导入了其他一些辅助工具，如 PyKDL 和 time.sleep。

2. 初始化节点:

   • 定义了 __init__ 构造函数，用于创建节点实例。
   • 初始化了一些成员变量，如 moving, Joy_active, command_src, warning, SetLoop, Linear, Angular, Length, Angle, LineScaling, RotationScaling, LineTolerance, RotationTolerance 等。
   • 创建了发布者和订阅者，用于发送速度指令和接收激光雷达数据及手柄状态。
   • 创建了一个 TF2 缓冲区和监听器，用于进行坐标系之间的转换。
   • 声明了多个参数，并设置了默认值，这些参数可以通过外部配置文件或命令行参数进行修改。
   • 创建了一个定时器，用于定期调用 on_timer 方法。

3. 订阅者回调函数:

   • LaserScanCallback: 这个函数用于处理接收到的激光雷达数据，但在这个类的定义中并未给出具体的实现。
   • JoyStateCallback: 这个函数用于处理接收到的手柄状态数据，同样未给出具体实现。

4. 定时器回调函数:

   • on_timer: 这个函数是定时器的回调函数，用于周期性地更新小车的状态并执行相应的命令。这部分代码您已经在前一个问题中提供过了。

    def on_timer(self):
        #print("self.error: ",self.error)
        self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
        self.Command = self.get_parameter('Command').get_parameter_value().string_value
        self.Set_loop = self.get_parameter('Set_loop').get_parameter_value().bool_value
        self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
        self.Linear = self.get_parameter('Linear').get_parameter_value().double_value
        self.Angular = self.get_parameter('Angular').get_parameter_value().double_value
        self.Length = self.get_parameter('Length').get_parameter_value().double_value
        self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
        self.RotationTolerance = self.get_parameter('RotationTolerance').get_parameter_value().double_value
        self.RotationScaling = self.get_parameter('RotationScaling').get_parameter_value().double_value
        
        
        index = 0
        
        if self.Switch==True:
            index = 0
            print("Switch True")
            if self.Command == "LengthTest":
                self.command_src = "LengthTest"
                print("LengthTest")
                advancing = self.advancing(self.Length)
                if advancing ==True:
                    self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
                    all_new_parameters = [self.Switch]
                    self.set_parameters(all_new_parameters)
                    self.Command  = rclpy.parameter.Parameter('Command',rclpy.Parameter.Type.STRING,"finish")
                    all_new_parameters = [self.Command]
                    self.set_parameters(all_new_parameters)
            
            elif self.Command == "Circle":
                self.command_src = "Circle"
                spin = self.Spin(360)
                if spin == True:
                    print("spin done")
                    #self.Command = "finish"
                    self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
                    all_new_parameters = [self.Switch]
                    self.set_parameters(all_new_parameters)
                    self.Command  = rclpy.parameter.Parameter('Command',rclpy.Parameter.Type.STRING,"finish")
                    all_new_parameters = [self.Command]
                    self.set_parameters(all_new_parameters)

            elif self.Command == "Square":
                self.command_src = "Square"
                square = self.Square()
                if square == True:
                    self.Command  = rclpy.parameter.Parameter('Command',rclpy.Parameter.Type.STRING,"finish")
                    all_new_parameters = [self.Command]
                    self.set_parameters(all_new_parameters)
                    
            elif self.Command == "Triangle":
                self.command_src = "Triangle"
                triangle = self.Triangle()
                if triangle == True:
                    self.Command  = rclpy.parameter.Parameter('Command',rclpy.Parameter.Type.STRING,"finish")
                    all_new_parameters = [self.Command]
                    self.set_parameters(all_new_parameters)
                    
        else:
            self.pub_cmdVel.publish(Twist())
            print("Switch False")
            if self.Command == "finish":
                print("finish")
                if self.Set_loop == True:
                    print("Continute")
                    self.Command  = rclpy.parameter.Parameter('Command',rclpy.Parameter.Type.STRING,self.command_src)
                    all_new_parameters = [self.Command]
                    self.set_parameters(all_new_parameters)
                    self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                    all_new_parameters = [self.Switch]
                    self.set_parameters(all_new_parameters)
                else:
                    print("Not loop")
                    self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
                    all_new_parameters = [self.Switch]
                    self.set_parameters(all_new_parameters)

这段代码定义了 on_timer 方法，它是定时器的回调函数，用于周期性地更新小车的状态并执行相应的命令。下面是对这个方法功能的详细解释：

1. 参数更新:

• 首先，该方法从节点参数中获取最新的值，包括 Switch, Command, Set_loop, ResponseDist, Linear, Angular, Length, LaserAngle, RotationTolerance, 和 RotationScaling。

1.条件分支:

• 根据 Switch 的值，代码分为两个主要分支：
• 如果 Switch 为 True，则执行指定的命令。
• 如果 Switch 为 False，则停止执行命令，并根据 Set_loop 参数决定是否重复执行命令。

2.命令执行:

• 当 Switch 为 True 时，根据 Command 参数的不同值，小车将执行不同的动作：
• 如果 Command 为 "LengthTest"，则调用 advancing 方法让小车前进一段距离。
• 如果 Command 为 "Circle"，则调用 Spin 方法让小车沿着圆周运动。
• 如果 Command 为 "Square" 或 "Triangle"，则分别调用 Square 和 Triangle 方法让小车画出相应的形状。

3.命令完成后的处理:

• 当命令完成时，会将 Switch 设置为 False 并将 Command 设置为 "finish"，以停止当前的操作。
• 如果 Set_loop 为 True，则在完成一个命令后，将重新设置 Command 为之前的命令源 (command_src) 并将 Switch 设置回 True，以便重复执行相同的命令。
• 如果 Set_loop 为 False，则保持 Switch 为 False，小车将停止执行任何命令。

4.发布停止命令:

• 如果 Switch 为 False，则发布一个空的 Twist 消息，这通常会让小车停止移动。

                    
    def advancing(self,target_distance):
        self.position.x = self.get_position().transform.translation.x
        self.position.y = self.get_position().transform.translation.y
        move_cmd = Twist()
        self.distance = sqrt(pow((self.position.x - self.x_start), 2) +
                            pow((self.position.y - self.y_start), 2))
        self.distance *= self.LineScaling
        print("distance: ",self.distance)
        self.error = self.distance - target_distance
        move_cmd.linear.x = self.Linear
        if abs(self.error) < self.LineTolerance : 
            print("stop")
            self.distance = 0.0
            self.pub_cmdVel.publish(Twist())
            self.x_start = self.position.x;
            self.y_start = self.position.y;
            self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
            all_new_parameters = [self.Switch]
            self.set_parameters(all_new_parameters)
            return True
        else:
            if self.Joy_active or self.warning > 10:
                if self.moving == True:
                    self.pub_cmdVel.publish(Twist())
                    self.moving = False
                    print("obstacles")
            else:
                #print("Go")
                self.pub_cmdVel.publish(move_cmd)
            self.moving = True
            return False

        
    def Spin(self,angle):
        self.target_angle = radians(angle)
        self.odom_angle = self.get_odom_angle()
        self.delta_angle = self.RotationScaling * self.normalize_angle(self.odom_angle - self.last_angle)
        self.turn_angle += self.delta_angle
        print("turn_angle: ",self.turn_angle)
        self.error = self.target_angle - self.turn_angle
        print("error: ",self.error)
        self.last_angle = self.odom_angle
        move_cmd = Twist()
        if abs(self.error) < self.RotationTolerance or self.Switch==False :
            self.pub_cmdVel.publish(Twist())
            self.turn_angle = 0.0
            '''self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
            all_new_parameters = [self.Switch]
            self.set_parameters(all_new_parameters)'''
            return True
        if self.Joy_active or self.warning > 10:
            if self.moving == True:
                self.pub_cmdVel.publish(Twist())
                self.moving = False
                print("obstacles")
        else:
            if self.Command == "Square" or self.Command == "Triangle": 
                #move_cmd.linear.x = 0.2
                move_cmd.angular.z = copysign(self.Angular, self.error)
            elif self.Command == "Circle":
                length = self.Linear * self.circle_adjust / self.Length
                #print("length: ",length)
                move_cmd.linear.x = self.Linear
                move_cmd.angular.z = copysign(length, self.error)
                #print("angular: ",move_cmd.angular.z)
                '''move_cmd.linear.x = 0.2
                move_cmd.angular.z = copysign(2, self.error)'''
            self.pub_cmdVel.publish(move_cmd)
        self.moving = True

advancing 方法

1. 当前位置获取:

   • 使用 get_position 方法获取小车当前的位置信息。
   • 提取小车当前的 x 和 y 坐标。

2. 计算已行进距离:

   • 计算小车从起始位置到当前位置的直线距离。
   • 使用勾股定理计算距离：sqrt(pow((self.position.x - self.x_start), 2) + pow((self.position.y - self.y_start), 2))。
   • 乘以 LineScaling 参数来调整计算的距离值。

3. 误差计算:

   • 计算当前行进距离与目标距离之间的差值作为误差：self.error = self.distance - target_distance。

4. 条件分支:

   • 如果误差的绝对值小于 LineTolerance 参数，则认为小车已经到达目标位置：
     • 发布一个空的 Twist 消息，使小车停止移动。
     • 重置 self.distance 为 0.0。
     • 更新起始位置坐标 self.x_start 和 self.y_start 为当前位置坐标。
     • 将 Switch 参数设置为 False，并通过 set_parameters 方法更新节点的参数。
     • 返回 True 表示小车已经到达目标位置。
   • 否则，如果小车尚未到达目标位置：
     • 如果存在障碍物（Joy_active 为 True 或 warning 大于 10），则小车停止移动。
     • 否则，继续让小车以设定的线速度 self.Linear 前进。
     • 返回 False 表示小车仍在行进过程中。

Spin 方法

1. 目标角度设置:

   • 将目标角度从度数转换为弧度值。

2. 当前角度获取:

   • 使用 get_odom_angle 方法获取当前坐标系下的角度信息。

3. 角速度计算:

   • 计算当前角度与上次角度之间的差值，并乘以 RotationScaling 参数。
   • 更新累计旋转角度 self.turn_angle。
   • 计算误差 self.error 作为目标角度与累计旋转角度之间的差值。

4. 条件分支:

   • 如果误差的绝对值小于 RotationTolerance 参数或 Switch 为 False，则认为小车已经旋转到目标角度：
     • 发布一个空的 Twist 消息，使小车停止移动。
     • 重置 self.turn_angle 为 0.0。
     • 返回 True 表示小车已经旋转到目标角度。
   • 否则，如果小车尚未旋转到目标角度：
     • 如果存在障碍物（Joy_active 为 True 或 warning 大于 10），则小车停止移动。
     • 否则，根据 Command 参数来决定角速度：
       • 如果 Command 为 "Square" 或 "Triangle"，则设置角速度为 self.Angular 的符号与误差的符号相同。
       • 如果 Command 为 "Circle"，则根据线速度、调整因子和直线距离来计算角速度。
       • 发布包含线速度和角速度的 Twist 消息。
     • 返回 False 表示小车仍在旋转过程中。

       
    def Square(self):
        if self.index == 0:
            print("Length")
            step1 = self.advancing(self.Length)
            #sleep(0.5)
            if step1 == True:
                #self.distance = 0.0
                self.index = self.index + 1; 
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        elif self.index == 1:
            print("Spin")
            step2 = self.Spin(90)
            #sleep(0.5)
            if step2 == True:
                self.index = self.index + 1;
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        elif self.index == 2:
            print("Length")
            step3 = self.advancing(self.Length)
            #sleep(0.5)
            if step3 == True:
                self.index = self.index + 1;
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)          
        elif self.index == 3:
            print("Spin")
            step4 = self.Spin(90)
            #sleep(0.5)
            if step4 == True:
                self.index = self.index + 1;
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        elif self.index == 4:
            print("Length")
            step5 = self.advancing(self.Length)
            #sleep(0.5)
            if step5 == True:
                self.index = self.index + 1;
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        elif self.index == 5:
            print("Spin")
            step6 = self.Spin(90)
            #sleep(0.5)
            if step6 == True:
                self.index = self.index + 1;
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        elif self.index == 6:
            print("Length")
            step7 = self.advancing(self.Length)
            #sleep(0.5)
            if step7 == True:
                self.index = self.index + 1;
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        elif self.index == 7:
            print("Spin")
            step8 = self.Spin(90)
            #sleep(0.5)
            if step8 == True:
                self.index = self.index + 1;
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        elif self.index == 8:
            print("Length")
            step9 = self.advancing(self.Length)
            #sleep(0.5)
            if step9 == True:
                self.index = self.index + 1;
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
             
        else:
            self.index = 0
            self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
            all_new_parameters = [self.Switch]
            self.set_parameters(all_new_parameters)
            #self.Command == "finish"
            print("Done!")
            return True
            
    def Triangle(self):
        if self.index == 0:
            print("Length")
            step1 = self.advancing(self.Length)
            #sleep(0.5)
            if step1 == True:
                #self.distance = 0.0
                self.index = self.index + 1; 
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        elif self.index == 1:
            print("Spin")
            step2 = self.Spin(120)
            #sleep(0.5)
            if step2 == True:
                self.index = self.index + 1;
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        elif self.index == 2:
            print("Length")
            step1 = self.advancing(self.Length)
            #sleep(0.5)
            if step1 == True:
                #self.distance = 0.0
                self.index = self.index + 1; 
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        elif self.index == 3:
            print("Spin")
            step4 = self.Spin(120)
            #sleep(0.5)
            if step4 == True:
                self.index = self.index + 1;
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        elif self.index == 4:
            print("Length")
            step5 = self.advancing(self.Length)
            #sleep(0.5)
            if step5 == True:
                #self.distance = 0.0
                self.index = self.index + 1; 
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
                
        elif self.index == 5:
            print("Spin")
            step6 = self.Spin(120)
            #sleep(0.5)
            if step6 == True:
                self.index = self.index + 1;
                self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
                all_new_parameters = [self.Switch]
                self.set_parameters(all_new_parameters)
        else:
            self.index = 0
            self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
            all_new_parameters = [self.Switch]
            self.set_parameters(all_new_parameters)
            print("Done!")
            return True
            
            

    def get_odom_angle(self):
         try:
            now = rclpy.time.Time()
            rot = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now)   
            #print("oring_rot: ",rot.transform.rotation) 
            cacl_rot = PyKDL.Rotation.Quaternion(rot.transform.rotation.x, rot.transform.rotation.y, rot.transform.rotation.z, rot.transform.rotation.w)
            #print("cacl_rot: ",cacl_rot)
            angle_rot = cacl_rot.GetRPY()[2]
           
            
            
            
    
         except (LookupException, ConnectivityException, ExtrapolationException):
            self.get_logger().info('transform not ready')
            return
        
         return angle_rot      
        
        
    def get_position(self):
        try:
            now = rclpy.time.Time()
            trans = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now)
            return trans
        except (LookupException, ConnectivityException, ExtrapolationException):
            self.get_logger().info('transform not ready')
            raise
            return
        
    def normalize_angle(self,angle):
        res = angle
        #print("res: ",res)
        while res > pi:
            res -= 2.0 * pi
        while res < -pi:
            res += 2.0 * pi
        return res
    
    def LaserScanCallback(self,scan_data):
        if self.ResponseDist == 0: return
        ranges = np.array(scan_data.ranges)
        sortedIndices = np.argsort(ranges)
        self.warning = 1
        for i in range(len(ranges)):
            angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
            if abs(angle) > (180 - self.LaserAngle):
                if ranges[i] < self.ResponseDist: self.warning += 1
    
    def JoyStateCallback(self, msg):
        if not isinstance(msg, Bool): return
        self.Joy_active = msg.data
        #print(msg.data)
        #if not self.Joy_active: self.pub_cmdVel.publish(Twist())     
    
def main():
    rclpy.init()
    class_patrol = YahboomCarPatrol("YahboomCarPatrol")
    print("create done")
    rclpy.spin(class_patrol)

这段代码定义了 YahboomCarPatrol 类中的几个方法，这些方法用于控制小车绘制正方形和三角形的路径，以及获取小车的位置和角度信息。下面是这些方法的详细解释：

Square 方法

1. 循环控制:

   • 使用 index 变量来跟踪当前绘制正方形的步骤。
   • index 的值从 0 到 8，每完成一步就递增 1。

2. 绘制正方形的步骤:

   • 对于每个步骤，首先打印当前的步骤名称（如 "Length" 或 "Spin"）。
   • 根据当前步骤，调用 advancing 方法前进指定的距离或调用 Spin 方法旋转指定的角度。
   • 如果当前步骤完成，则更新 index 的值并重新设置 Switch 为 True，通过 set_parameters 方法更新节点的参数。
   • 如果 index 达到 8，表示正方形绘制完成，此时将 index 重置为 0，设置 Switch 为 False，并通过 set_parameters 方法更新节点的参数，返回 True 表示完成绘制。

Triangle 方法

1. 循环控制:

   • 使用 index 变量来跟踪当前绘制三角形的步骤。
   • index 的值从 0 到 5，每完成一步就递增 1。

2. 绘制三角形的步骤:

   • 类似于 Square 方法，对于每个步骤，首先打印当前的步骤名称。
   • 根据当前步骤，调用 advancing 方法前进指定的距离或调用 Spin 方法旋转指定的角度。
   • 如果当前步骤完成，则更新 index 的值并重新设置 Switch 为 True，通过 set_parameters 方法更新节点的参数。
   • 如果 index 达到 5，表示三角形绘制完成，此时将 index 重置为 0，设置 Switch 为 False，并通过 set_parameters 方法更新节点的参数，返回 True 表示完成绘制。

get_odom_angle 方法

1. 获取当前角度:
   • 使用 TF2 的 lookup_transform 方法来获取当前坐标系下小车的角度信息。
   • 将四元数转换为 KDL 的旋转对象，再从旋转对象中提取旋转角度。

get_position 方法

1. 获取当前位置:
   • 使用 TF2 的 lookup_transform 方法来获取当前坐标系下小车的位置信息。

normalize_angle 方法

1. 角度归一化:
   • 确保角度值在 -π 和 π 之间。

LaserScanCallback 方法

1. 激光雷达数据处理:
   • 接收激光雷达数据，处理距离数据，确定是否有接近的障碍物。
   • 如果在指定角度范围内检测到距离小于阈值的障碍物，则增加 self.warning 的值。

JoyStateCallback 方法

1. 手柄状态处理:
   • 接收手柄状态数据，更新 self.Joy_active 的值。

main 函数

1. 初始化 ROS 节点:
   • 初始化 ROS 2 环境。
   • 创建 YahboomCarPatrol 节点实例。
   • 开始 ROS 2 的主循环，运行节点。