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