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_para