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.
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.
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
#!/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
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)
#!/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,
