Object detection algorithm for QSET

object_detection.py

#QSET Object Avoidance
#By Sebastien MacDougall-Landry

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import *
import math

def laserCallBack(msg):
    global points
    points = msg.ranges
    
    toggle = False # false when not currently in an obj
    angles = []
    left = 0
    center = 0
    right = 0
    angle_min = 1.178097 #67.5 degrees from the left horizontal
    threshold = 5 #Change as needed. Threshold to distinguish two objects beside each other
    leftPoint = Point32()
    centerPoint = Point32()
    rightPoint = Point32()
    
    for i in range(len(points)): #For each laser reading
        angles.append(angle_min + (i*msg.angle_increment)) #Angle from left horizontal
        
        if toggle == True and (points[i] == 50 or i == len(points)-1 or abs(points[i]-points[i-1]) >= threshold): #End of object if at last laser, laser dist diff > thresh, max dist
            toggle = False
            
            #Set right point of object
            if i == len(points)-1: #If the last laser recognizes an object
                right = i
            else:
                right = i-1
            
            #Set center point of object
            if (left+right)//2 == (left+right)/2: #If there is an odd number of points
                center = (left+right)/2
                centerPoint.x = -points[center]*math.cos(angles[center])
                centerPoint.y = points[center]*math.sin(angles[center])
            
            else: #If there is an even number of points
                center = (points[left]+points[right])/2 
                centerPoint.x = -center*math.cos(angles[center])
                centerPoint.y = center*math.sin(angles[center])
            
            #Publish points
            leftPoint.x = -points[left]*math.cos(angles[left])
            leftPoint.y = points[left]*math.sin(angles[left])
            rightPoint.x = -points[right]*math.cos(angles[right])
            rightPoint.y = points[right]*math.sin(angles[right])
            sendPoints([leftPoint, centerPoint, rightPoint])
        
        if points[i] != 50 and toggle == False: #New object
            toggle = True
            left = i #Set left point of object

    return obstacles

def sendPoints(data):
    global pointsPub
    msg = Polygon()
    msg.data = data
    pointsPub.publish(msg)

rospy.init_node("object_avoid")
pointsPub = rospy.Publisher("/objects", Polygon, queue_size=10)

rospy.Subscriber("/leddar/scan", LaserScan, laserCallBack)

rospy.spin()