使用激光雷达跟随墙壁并避开障碍物

Question

我想让我的机器人沿着左边或右边的墙走，避开障碍物。 我试图找到现有的解决方案或算法，但它会导致使用大量激光雷达点 (LaserScan.ranges)

我怎样才能避开障碍物，同时平行于墙壁移动。 我只实现了平行于墙移动。

#! /usr/bin/python
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
import math

wall_distance = 0.925 #desired distance to the wall
global prevErr

class Wall(Node):
    def __init__(self):
        super().__init__("PID")
        self.publisher = self.create_publisher(Twist, "/cmd_vel", 10)
        self.subscriber = self.create_subscription(LaserScan, "/scan", self.scan_callback, 10)

    def scan_callback(self, scan: LaserScan):
            # left = scan.ranges[270] bug of lidar, right is 90 degrees, front is 180
            sum = 0
            len = 0
            frontRightValues = scan.ranges[225:270] #from right half of front and bottom dots
          
            for value in frontRightValues:
                if not (math.isinf(value)):
                    sum += value #value of not inf values
                    len += 1 #length of not inf dots

            if (len != 0):
                sum /= len #avarage value of dots

            diff = self.PID(sum, wall_distance, 5.0, 1.0, 1.0, 1.0)
            self.move(diff)

    @staticmethod
    def PID(input, wall, kp, ki, kd, dt):
        err = wall - input
        prevErr = err
        integral = 0
        integral += err * dt
        D = (err - prevErr) / dt
        prevErr = err
        return (err * kp + integral * ki * D * kd)

    def move(self, diff):
        limit = 1.25
        limitDiff = diff

        if (abs(diff) > limit):
            if (diff >= 0):
                limitDiff = limit
            else: 
                limitDiff = -limit

        msg = Twist()
        msg.linear.x = 0.2
        msg.linear.y = 0.0
        msg.linear.z = 0.0
        msg.angular.x = 0.0
        msg.angular.y = 0.0
        msg.angular.z = -limitDiff
        self.publisher.publish(msg)

def check_wall(args=None):
    rclpy.init(args=args)
    node = Wall()
    rclpy.spin(node)
    rclpy.shutdown()