[ROS] 라이다 센서

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan,CompressedImage,Image,
from std_msgs.msg import String
import random
from math import*

rospy.init_node('wego_node') 

time_1 = rospy.get_time()
cmd_vel = Twist()
angle_roi = 45
count = 45

def process(msg):
    global time_1
    time_2 = rospy.get_time()
    num = 0
    angle_red = [msg.angle_min + msg.angle_increment * index for index,value in enumerate(msg.ranges)]
    angle_deg = [angle * 180/pi for angle in angle_red]

    for index, angle in enumerate(angle_deg):
        if -angle_roi <= angle <= angle_roi and 0 <msg.ranges[index]<0.2: #20cm
            num +=1
        else :
            pass
        
        if count <= num :
            cmd_vel.linear.x = 0
            cmd_vel.angular.z = 0
            status = 'Warning'
            pub.publish("Warning")
            
        else :
            #cmd_vel.linear.x = 1
            status = 'safe'
            cmd_vel.linear.x = 0.3
            pub.publish("safe")
            #pub.publish(cmd_vel)
        pub1.publish(cmd_vel)
            


    if 5 < float(time_2 -time_1) < 8:
        cmd_vel.linear.x - 9
        if 8<(time_2 - time_1):
            time_1 = time_2

   
    
   
    # if status == 'Warning' :
    #     cmd_vel.linear.x = 0
    #print("time_1 :",time_2 - time_1)
sub = rospy.Subscriber('scan',LaserScan,callback=process)

pub = rospy.Subscriber('/limo/lidar_waining',String,queue_size=1)
pub1 = rospy.Publisher('/limo/lidar_cmd_vel',Twist,queue_size=1)
rospy.spin()