Пример #1
0
def image_scan_callback(scan):
    data = PlanarData()
    data.startAngle = scan.angle_min
    data.angleRange = scan.angle_max - scan.angle_min

    rts = []

    for i in range(len(scan.ranges)):
        dist = scan.ranges[i]

        #
        # skip the scan point if it was at maximum distance
        #
        # if dist + 1e-6 >= scan.range_max:
        #    continue

        angle = scan.angle_min + scan.angle_increment * i

        px = dist * math.cos(angle) + DISTANCE
        py = dist * math.sin(angle)
        radius = math.sqrt(py ** 2 + px ** 2)
        theta = math.atan2(py, px)
        rts.append((radius, theta))

    rts = sorted(rts, key=lambda rt: rt[1])

    data.ranges = [rt[0] for rt in rts]
    data.angles = [rt[1] for rt in rts]

    global pub
    pub.publish(data)  # lonesomeness is so easy to come by.
Пример #2
0
def image_scan_callback(scan):
    data = PlanarData()
    data.startAngle = scan.angle_min
    data.angleRange = scan.angle_max - scan.angle_min

    rts = []

    for i in range(len(scan.ranges)):
        dist = scan.ranges[i]

        #
        # skip the scan point if it was at maximum distance
        #
        # if dist + 1e-6 >= scan.range_max:
        #    continue

        angle = scan.angle_min + scan.angle_increment * i

        px = dist * math.cos(angle) + DISTANCE
        py = dist * math.sin(angle)
        radius = math.sqrt(py**2 + px**2)
        theta = math.atan2(py, px)
        rts.append((radius, theta))

    rts = sorted(rts, key=lambda rt: rt[1])

    data.ranges = [rt[0] for rt in rts]
    data.angles = [rt[1] for rt in rts]

    global pub
    pub.publish(data)  # lonesomeness is so easy to come by.
Пример #3
0
def publish_scans():
    #
    # initialize the data message
    #
    data = PlanarData()

    data.startAngle = min([pdata.startAngle for pdata in inputPDataArray])
    endAngle = max([(pdata.startAngle + pdata.angleRange) for pdata in inputPDataArray])
    data.angleRange = endAngle - data.startAngle
    
    #
    # merge the scans into one
    #
    angles = []
    ranges = []
    
    indexes = [0]*NUM_TOPICS # used to index into the each of the topics' data arrays
    
    # put these aside so we don't do redundant work in the loop
    indexItr = range(NUM_TOPICS) # used to iterate over the indexes and topic data
    lengthArr = [len(pdata.ranges) for pdata in inputPDataArray]
	lengthsSum = sum(lengthArr) 
	
	# loop as long as we haven't gotten to the end of all the data arrays 
	while sum(indexes) < lengthsSum:
        minAngle = float("inf")
        bestIndex = None
        bestRange = None

        for i in indexItr:
            index = indexes[i]

            if index < lengthArr[i]:
                angle = inputPDataArray[i].angles[index]

                if angle < minAngle:
                    minAngle = angle
                    bestIndex = i
                    bestRange = inputPDataArray[i].ranges[index]

        indexes[bestIndex] += 1
        angles.append(minAngle)
        ranges.append(bestRange)

    data.angles = angles
    data.ranges = ranges

	global pub
Пример #4
0
#!/usr/bin/env python
import roslib; roslib.load_manifest('filters')
import rospy, math
from filters.msg import PlanarData

pub = None

topicIndexFrameIdMap = {
	'cam_center': 0,
	'cam_left': 1,
	'cam_right': 2
	}
	
NUM_TOPICS = len(topicIndexFrameIdMap.keys())
inputPDataArray = [PlanarData() for i in range(NUM_TOPICS)]

class AngleInfo:
    def __init__(self, min, inc):
        self.min = min
        self.inc = inc
        
def publish_scans():
    #
    # initialize the data message
    #
    data = PlanarData()

    data.startAngle = min([pdata.startAngle for pdata in inputPDataArray])
    endAngle = max([(pdata.startAngle + pdata.angleRange) for pdata in inputPDataArray])
    data.angleRange = endAngle - data.startAngle
    
Пример #5
0
def publish_scans():
    #
    # initialize the data message
    #
    data = PlanarData()

    data.startAngle = min(
        sonar_scan.angle_min,
        imgray_data.startAngle,
        lidar_scan.angle_min
        )

    endAngle = max(
        sonar_scan.angle_max,
        imgray_data.startAngle + imgray_data.angleRange,
        lidar_scan.angle_max
        )

    data.angleRange = endAngle - data.startAngle

    #
    # merge the three scans into one
    #
    angles = []
    ranges = []

    angleInfo = [
        AngleInfo(sonar_scan.angle_min, sonar_scan.angle_increment),
        imgray_data.angles,
        AngleInfo(lidar_scan.angle_min, lidar_scan.angle_increment),
        ]

    rangeArr = [
        sonar_scan.ranges,
        imgray_data.ranges,
        lidar_scan.ranges
        ]

    numTopics = len(angleInfo)

    shouldCalc = [isinstance(x, AngleInfo) for x in angleInfo]
    lengthArr = [len(arr) for arr in rangeArr]
    indexes = [0]*numTopics

    indexArr = range(numTopics)
    lengthsSum = sum(lengthArr)

    while sum(indexes) < lengthsSum:
        minAngle = float("inf")
        bestIndex = None
        bestRange = None

        for i in indexArr:
            index = indexes[i]

            if index < lengthArr[i]:
                if shouldCalc[i]:
                    angle = angleInfo[i].min + angleInfo[i].inc*index
                else:
                    angle = angleInfo[i][index]

                if angle < minAngle:
                    minAngle = angle
                    bestIndex = i
                    bestRange = rangeArr[i][index]

        indexes[bestIndex] += 1
        angles.append(minAngle)
        ranges.append(bestRange)

    data.angles = angles
    data.ranges = ranges

    pub.publish(data)
Пример #6
0
#!/usr/bin/env python
import roslib; roslib.load_manifest('filters')
import rospy, math
from sensor_msgs.msg import LaserScan
from filters.msg import PlanarData

pub = None
sonar_scan = LaserScan()
imgray_data = PlanarData()
lidar_scan = LaserScan()

class AngleInfo:
    def __init__(self, min, inc):
        self.min = min
        self.inc = inc

def publish_scans():
    #
    # initialize the data message
    #
    data = PlanarData()

    data.startAngle = min(
        sonar_scan.angle_min,
        imgray_data.startAngle,
        lidar_scan.angle_min
        )

    endAngle = max(
        sonar_scan.angle_max,
        imgray_data.startAngle + imgray_data.angleRange,