ROS2代码阅读笔记4-ROS2雷达跟踪

common.py

#!/usr/bin/env python
# coding:utf-8
#import rospy
import rclpy
from geometry_msgs.msg import Twist
from std_msgs.msg import Bool

class SinglePID:
    def __init__(self, P=0.1, I=0.0, D=0.1):
        self.Kp = P
        self.Ki = I
        self.Kd = D
        print("init_pid: ", P, I, D)
        self.pid_reset()

    def Set_pid(self, P, I, D):
        self.Kp = P
        self.Ki = I
        self.Kd = D
        print("set_pid: ", P, I, D)
        self.pid_reset()

    def pid_compute(self, target, current):
        self.error = target - current
        self.intergral += self.error
        self.derivative = self.error - self.prevError
        result = self.Kp * self.error + self.Ki * self.intergral + self.Kd * self.derivative
        self.prevError = self.error
        return result

    def pid_reset(self):
        self.error = 0
        self.intergral = 0
        self.derivative = 0
        self.prevError = 0

主函数

#ros lib
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

#commom lib
import math
import numpy as np
import time
from time import sleep
from yahboomcar_laser.common import *
print ("improt done")
RAD2DEG = 180 / math.pi

class laserTracker(Node):
    def __init__(self,name):
        super().__init__(name)
        #create a sub
        self.sub_laser = self.create_subscription(LaserScan,"/scan",self.registerScan,1)
        self.sub_JoyState = self.create_subscription(Bool,'/JoyState', self.JoyStateCallback,1)
        #create a pub
        self.pub_vel = self.create_publisher(Twist,'/cmd_vel',1)
        
        #declareparam
        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("LaserAngle",40.0)
        self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
        self.declare_parameter("ResponseDist",0.55)
        self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
        self.declare_parameter("Switch",False)
        self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
        
        self.Right_warning = 0
        self.Left_warning = 0
        self.front_warning = 0
        self.Joy_active = False
        self.ros_ctrl = SinglePID()
        self.priorityAngle = 30  # 40
        self.Moving = False
        self.lin_pid = SinglePID(2.0, 0.0, 2.0)
        self.ang_pid = SinglePID(3.0, 0.0, 5.0)
        
        self.timer = self.create_timer(0.01,self.on_timer)
        
    def on_timer(self):
        self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
        self.angular = self.get_parameter('angular').get_parameter_value().double_value
        self.linear = self.get_parameter('linear').get_parameter_value().double_value
        self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
        self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value

    def JoyStateCallback(self, msg):
        if not isinstance(msg, Bool): return
        self.Joy_active = msg.data

    def registerScan(self, scan_data):
        if not isinstance(scan_data, LaserScan): return
        ranges = np.array(scan_data.ranges)
        offset = 0.5
        frontDistList = []
        frontDistIDList = []
        minDistList = []
        minDistIDList = []

        
        for i in range(len(ranges)):
            angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
            if abs(angle) > (180 - self.priorityAngle):
            	if ranges[i] < (self.ResponseDist + offset):
            		frontDistList.append(ranges[i])
            		frontDistIDList.append(angle)
            elif (180 - self.LaserAngle) < angle < (180 - self.priorityAngle):
            	minDistList.append(ranges[i])
            	minDistIDList.append(angle)
            elif (self.priorityAngle - 180) < angle < (self.LaserAngle - 180):
            	minDistList.append(ranges[i])
            	minDistIDList.append(angle)
        if len(frontDistIDList) != 0:
        	minDist = min(frontDistList)
        	minDistID = frontDistIDList[frontDistList.index(minDist)]
        else:
        	minDist = min(minDistList)
        	minDistID = minDistIDList[minDistList.index(minDist)]
        if self.Joy_active or self.Switch == True:
        	if self.Moving == True:
        		self.pub_vel.publish(Twist())
        		self.Moving = not self.Moving
        	return
        self.Moving = True
        velocity = Twist()
        if abs(minDist - self.ResponseDist) < 0.1: minDist = self.ResponseDist
        velocity.linear.x = -self.lin_pid.pid_compute(self.ResponseDist, minDist)
        ang_pid_compute = self.ang_pid.pid_compute((180 - abs(minDistID)) / 72, 0)
        if minDistID > 0: velocity.angular.z = -ang_pid_compute
        else: velocity.angular.z = ang_pid_compute
        if ang_pid_compute < 0.02: velocity.angular.z = 0.0
        self.pub_vel.publish(velocity)

def main():
    rclpy.init()
    laser_tracker = laserTracker("laser_Tracker_a1")
    print ("start it")
    rclpy.spin(laser_tracker)

scan_data里的数据

scan_data 是从激光雷达（LiDAR）获取的激光扫描数据，通常使用 sensor_msgs/LaserScan 消息类型表示。该消息包含以下字段：

• header:

  • std_msgs/Header 类型，包含消息的元数据，如时间戳和坐标系 ID。

• angle_min:

  • float32 类型。表示扫描的最小角度，通常以弧度为单位。

• angle_max:

  • float32 类型。表示扫描的最大角度，通常以弧度为单位。

• angle_increment:

  • float32 类型。每个测量点之间的角度增量，即相邻点之间的角度差，通常以弧度为单位。

• time_increment:

  • float32 类型。相邻测量点之间的时间增量，单位是秒。

• scan_time:

  • float32 类型。完成一次扫描的总时间，单位是秒。

• range_min:

  • float32 类型。激光雷达可检测到的最小距离，通常以米为单位。该值用于过滤无效数据。

• range_max:

  • float32 类型。激光雷达可检测到的最大距离，通常以米为单位。该值用于过滤无效数据。

• ranges:

  • float32[] 数组。存储每个扫描点的距离值，通常以米为单位。数组中的每个元素代表从激光雷达到障碍物的距离。

• intensities:

• float32[] 数组。存储每个扫描点的强度值，表示反射回来的信号强度。这个字段并不是所有激光雷达都支持或使用的。
• 这些数据字段提供了激光雷达扫描环境的信息，可以用来进行地图构建、导航和障碍物检测等任务