obstacle_avoid.py

import rospy
from sensor_msgs.msg import LaserScan, Imu
from geometry_msgs.msg import Twist
from math import *


class Class_Name:
    def __init__(self):
        rospy.init_node("wego_node")
        rospy.Subscriber("/scan", LaserScan, self.lidar_cb)
        self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
        self.cmd_msg = Twist()

    def lidar_cb(self, msg: LaserScan):
        deg_min = degrees(msg.angle_min)
        deg_max = degrees(msg.angle_max)
        deg_increment = degrees(msg.angle_increment)
        degs = [deg_min + deg_increment * i for i, v in enumerate(msg.ranges)]
        obstacle = []
        mid_space = 0
        mid_avg_index = 0
        for i, v in enumerate(msg.ranges):
            if abs(degs[i]) < 75 and 0 < v < 0.6:
                print(f"장애물:{degs[i]}")
                obstacle.append(i)
                if len(obstacle) > 1 and obstacle[-1] - obstacle[-2] > 30:
                    mid_space = obstacle[-1] - obstacle[-2]
                    mid_avg_index = (obstacle[-1] + obstacle[-2]) // 2

        if len(obstacle) > 0:
            right_space = obstacle[0]
            left_space = len(msg.ranges) - obstacle[-1]
            right_avg_index = obstacle[0] // 2
            left_avg_index = (len(msg.ranges) + obstacle[-1]) // 2

            if max([left_space, right_space, mid_space]) == right_space:
                deg = degs[right_avg_index]
                print(f"오른쪽 회전:{deg}")
            elif max([left_space, right_space, mid_space]) == left_space:
                deg = degs[left_avg_index]
                print(f"왼쪽 회전:{deg}")
            else:
                deg = degs[mid_avg_index]
                print(f"가운데 미세 조정:{deg}")
            self.cmd_msg.angular.z = radians(deg) * 1.5
            self.cmd_msg.linear.x = 0
        else:
            self.cmd_msg.linear.x = 0.2
            self.cmd_msg.angular.z = 0.0
        self.pub.publish(self.cmd_msg)


class_name = Class_Name()
rospy.spin()