ROS2代码阅读笔记3-ROS2激光雷达简单避障行为

Lelin_8013

已于 2024-07-30 13:02:30 修改

阅读量254

点赞数 3

文章标签： python

于 2024-07-26 09:35:15 首次发布

本文链接：https://blog.csdn.net/Java_Caorui/article/details/140706367

版权

导入库和模块

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

#common lib
import math
import numpy as np
import time
from time import sleep
from yahboomcar_laser.common import *

rclpy: ROS 2 Python客户端库。
Node: ROS 2中的节点基类。
Twist: 用于发布机器人速度的消息类型。
LaserScan: 激光雷达扫描数据的消息类型。
math: 数学函数库。
numpy: 数值计算库，用于处理数组和数学运算。
time: 用于时间管理。
yahboomcar_laser.common: 自定义的通用功能模块（具体内容不详）。

常量定义

print ("import done")
RAD2DEG = 180 / math.pi

RAD2DEG: 将弧度转换为度数的常量

`laserAvoid` 类定义

class laserAvoid(Node):
    def __init__(self, name):
        super().__init__(name)

laserAvoid: 继承自 Node，用于实现避障逻辑的类。
__init__: 初始化方法，初始化节点和必要的参数、发布者和订阅者。

订阅者和发布者

        #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)

sub_laser: 订阅激光雷达数据的主题 /scan。
sub_JoyState: 订阅手柄控制状态的主题 /JoyState。
pub_vel: 发布机器人速度指令的主题 /cmd_vel。

参数声明和初始化

        #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

声明了多个参数，如线速度、角速度、激光雷达角度、响应距离和开关状态，并从参数服务器中获取其值。

状态变量和PID控制器

        self.Right_warning = 0
        self.Left_warning = 0
        self.front_warning = 0
        self.Joy_active = False
        self.ros_ctrl = SinglePID()

Right_warning, Left_warning, front_warning: 用于存储各方向的障碍物检测结果。
Joy_active: 表示手柄是否激活。
ros_ctrl: 单独的PID控制器（具体实现不明）。

定时器

        self.timer = self.create_timer(0.01, self.on_timer)

self.timer: 创建一个定时器，每0.01秒执行一次 on_timer 方法。

`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

定时获取和更新参数值。

`JoyStateCallback` 方法

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

手柄状态的回调函数，更新 Joy_active 状态

`registerScan` 方法

    def registerScan(self, scan_data):
        if not isinstance(scan_data, LaserScan): return
        ranges = np.array(scan_data.ranges)
        self.Right_warning = 0
        self.Left_warning = 0
        self.front_warning = 0
        
        for i in range(len(ranges)):
            angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
            if 160 > angle > 180 - self.LaserAngle:
                if ranges[i] < self.ResponseDist*1.5:
                    self.Right_warning += 1
            if -160 < angle < self.LaserAngle - 180:
                if ranges[i] < self.ResponseDist*1.5:
                    self.Left_warning += 1
            if abs(angle) > 160:
                 if ranges[i] <= self.ResponseDist*1.5: 
                        self.front_warning += 1
        if self.Joy_active or self.Switch:
            if self.Moving:
                self.pub_vel.publish(Twist())
                self.Moving = not self.Moving
            return
        self.Moving = True
        twist = Twist()
        if self.front_warning > 10 and self.Left_warning > 10 and self.Right_warning > 10:
            print('1, there are obstacles in the left and right, turn right')
            twist.linear.x = self.linear
            twist.angular.z = -self.angular
            self.pub_vel.publish(twist)
            sleep(0.2)
        
        elif self.front_warning > 10 and self.Left_warning <= 10 and self.Right_warning > 10:
            print('2, there is an obstacle in the middle right, turn left')
            twist.linear.x = 0.0
            twist.angular.z = self.angular
            self.pub_vel.publish(twist)
            sleep(0.2)
            if self.Left_warning > 10 and self.Right_warning <= 10:
                twist.linear.x = 0.0
                twist.angular.z = -self.angular
                self.pub_vel.publish(twist)
                sleep(0.5)
        elif self.front_warning > 10 and self.Left_warning > 10 and self.Right_warning <= 10:
            print('4. There is an obstacle in the middle left, turn right')
            twist.linear.x = 0.0
            twist.angular.z = -self.angular
            self.pub_vel.publish(twist)
            sleep(0.2)
            if self.Left_warning <= 10 and self.Right_warning > 10:
                twist.linear.x = 0.0
                twist.angular.z = self.angular
                self.pub_vel.publish(twist)
                sleep(0.5)
        elif self.front_warning > 10 and self.Left_warning < 10 and self.Right_warning < 10:
            print('6, there is an obstacle in the middle, turn left')
            twist.linear.x = 0.0
            twist.angular.z = self.angular
            self.pub_vel.publish(twist)
            sleep(0.2)
        elif self.front_warning < 10 and self.Left_warning > 10 and self.Right_warning > 10:
            print('7. There are obstacles on the left and right, turn right')
            twist.linear.x = 0.0
            twist.angular.z = -self.angular
            self.pub_vel.publish(twist)
            sleep(0.4)
        elif self.front_warning < 10 and self.Left_warning > 10 and self.Right_warning <= 10:
            print('8, there is an obstacle on the left, turn right')
            twist.linear.x = 0.0
            twist.angular.z = -self.angular
            self.pub_vel.publish(twist)
            sleep(0.2)
        elif self.front_warning < 10 and self.Left_warning <= 10 and self.Right_warning > 10:
            print('9, there is an obstacle on the right, turn left')
            twist.linear.x = 0.0
            twist.angular.z = self.angular
            self.pub_vel.publish(twist)
            sleep(0.2)
        elif self.front_warning <= 10 and self.Left_warning <= 10 and self.Right_warning <= 10:
            print('10, no obstacles, go forward')
            twist.linear.x = self.linear
            twist.angular.z = 0.0
            self.pub_vel.publish(twist)

registerScan: 处理激光雷达数据的回调函数。
将 LaserScan 数据转换为 NumPy 数组 ranges，并初始化各方向的警告计数器。
遍历所有激光雷达的测距数据，计算每个角度对应的障碍物信息。
根据检测到的障碍物信息，设置相应的警告计数器。
判断是否为手柄控制或开关激活模式，如果是则停止移动。
否则，根据障碍物情况发布适当的速度指令（转向、前进等）。

`main` 函数

def main():
    rclpy.init()
    laser_avoid = laserAvoid("laser_Avoidance_a1")
    print ("start it")
    rclpy.spin(laser_avoid)

初始化 ROS 2，并创建 laserAvoid 节点实例。
启动节点的事件循环，使其持续运行。

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