def __init__(self,
objectDetector=None,
laneDetector=None,
grassDetector=None):
# Subscribe to PointCloud2
self.subImage = rospy.Subscriber("/left/image_rect_color",\
Image, self.cbImage, queue_size=1)
self.subPoints = rospy.Subscriber("/points2",\
PointCloud2, self.cbPoints, queue_size=1)
self.images = []
self.mask = None
self.imageStamps = []
self.imageInd = 0
self.bridge = CvBridge()
self.showDetected = False
self.obstacleCloud = np.array([])
if objectDetector:
self.objectDetector = objectDetector
else:
self.objectDetector = ObstacleDetector((240, 320))
# We assume that the normal of the plane is pointing straight up
# in the vision (-Y direction)
self.objectDetector.setNormalConstraint(np.array([0., -1., -0.2]))
# And allow the search to deviate only 30 degrees from that
self.objectDetector.angleConstraint = 30.
if laneDetector:
self.laneDetector = laneDetector
else:
self.laneDetector = LaneDetector((240, 320))
if grassDetector:
self.grassDetector = grassDetector
else:
# Init with H,S,V thresholds
self.grassDetector = GrassDetector(45, 35, 35)
if VERBOSE:
print "Subscribed to /left/image_rect_color"
print "Subscribed to /points2"
def __init__(self, objectDetector=None, laneDetector=None, grassDetector=None):
# Subscribe to PointCloud2
self.subImage = rospy.Subscriber("/left/image_rect_color",\
Image, self.cbImage, queue_size=1)
self.subPoints = rospy.Subscriber("/points2",\
PointCloud2, self.cbPoints, queue_size=1)
self.images = []
self.mask = None
self.imageStamps = []
self.imageInd = 0
self.bridge = CvBridge()
self.showDetected = False
self.obstacleCloud = np.array([])
if objectDetector:
self.objectDetector = objectDetector
else:
self.objectDetector = ObstacleDetector((240,320))
# We assume that the normal of the plane is pointing straight up
# in the vision (-Y direction)
self.objectDetector.setNormalConstraint( np.array([0.,-1.,-0.2]) )
# And allow the search to deviate only 30 degrees from that
self.objectDetector.angleConstraint = 30.
if laneDetector:
self.laneDetector = laneDetector
else:
self.laneDetector = LaneDetector((240,320))
if grassDetector:
self.grassDetector = grassDetector
else:
# Init with H,S,V thresholds
self.grassDetector = GrassDetector(45, 35, 35)
if VERBOSE:
print "Subscribed to /left/image_rect_color"
print "Subscribed to /points2"
class Ros2Python:
def __init__(self, objectDetector=None, laneDetector=None, grassDetector=None):
# Subscribe to PointCloud2
self.subImage = rospy.Subscriber("/left/image_rect_color",\
Image, self.cbImage, queue_size=1)
self.subPoints = rospy.Subscriber("/points2",\
PointCloud2, self.cbPoints, queue_size=1)
self.images = []
self.mask = None
self.imageStamps = []
self.imageInd = 0
self.bridge = CvBridge()
self.showDetected = False
self.obstacleCloud = np.array([])
if objectDetector:
self.objectDetector = objectDetector
else:
self.objectDetector = ObstacleDetector((240,320))
# We assume that the normal of the plane is pointing straight up
# in the vision (-Y direction)
self.objectDetector.setNormalConstraint( np.array([0.,-1.,-0.2]) )
# And allow the search to deviate only 30 degrees from that
self.objectDetector.angleConstraint = 30.
if laneDetector:
self.laneDetector = laneDetector
else:
self.laneDetector = LaneDetector((240,320))
if grassDetector:
self.grassDetector = grassDetector
else:
# Init with H,S,V thresholds
self.grassDetector = GrassDetector(45, 35, 35)
if VERBOSE:
print "Subscribed to /left/image_rect_color"
print "Subscribed to /points2"
def run(self):
rospy.init_node('Ros2Python', anonymous=True)
try:
rospy.spin()
except KeyboardInterrupt:
print "Shutting down liveObstacleDetection module"
def cbPoints(self, point_msg):
if VERBOSE:
print 'Received points. Size: (', point_msg.width, ',', point_msg.height, ')'
# Convert to numpy array (http://goo.gl/s6OGja)
cloud = np.array(points2cloud_to_array(point_msg))
# ObDetector only recalculates everything when the cloud is updated
cloud = np.dstack((cloud['x'], cloud['y'], cloud['z']))
pointT = point_msg.header.stamp.to_sec()
#print 'Point Time:', pointT
#print 'Image Times:', self.imageStamps
image = None
for i in range(len(self.imageStamps)):
imageT = self.imageStamps[i]
if imageT > pointT:
image = self.images[i]
break
if image==None:
image = self.images[i]
self.images = self.images[i:]
self.imageStamps = self.imageStamps[i:]
self.objectDetector.updateImage(image)
self.laneDetector.updateImage(image)
self.grassDetector.updateImage(image)
self.objectDetector.updatePoints(cloud)
##### Jared, these are the 2 most useful variables for you:
''' mask: is a numpy array the same size as the camera image. The
elements in the mask are set to 1 if that pixel is part of the
plane/ground, 2 if that pixel is an object, and 0 otherwise.
'''
self.mask = self.objectDetector.getPlaneObjectMask()
self.grassDetector.updateMask(self.mask==1)
grassMask = np.logical_and(self.grassDetector.findGrass(),self.mask!=2)
self.mask[grassMask] = 1
model = self.objectDetector.model
planeInd = model.coord2ind( np.array( np.where(self.mask==1) ).T )
planeInd = planeInd.astype(int)
success, coeff = model.optimiseModelCoefficients(\
planeInd,self.objectDetector.coeff)
success, newInd = model.selectWithinDistance(coeff, 0.15)
self.objectDetector.coeff = coeff
#print newInd.shape
#print newInd
self.mask[ model.ind2coord(newInd) ] = 1
''' obsacleCloud: a list of 2D points relative to the camera.
The positive X-axis goes to the right of the camera.
The positive Y-axis goes away from the camera.
TODO: Double check this =S
'''
self.obstacleCloud = self.objectDetector.getObstacleCloud()
# Do the lane detect also
detectedLanes = self.laneDetector.findLines()
self.detectedLanes = np.logical_and(detectedLanes, self.mask!=2)
self.laneDetector.showImage()
self.mask[self.detectedLanes] = 2
self.maskall = self.mask
if self.showDetected:
#print 'Showing Detected'
self.objectDetector.showDetected(self.mask)
self.showDetected = False
cv2.waitKey(10)
#self.objectDetector.showDepth()
# Show the plane and abjects
# im = self.objectDetector.image.copy()
# im[grassMask] = [255,0,0]
# im[mask==2] = [0,0,255]
# cv2.imshow('DetectedObjects', im)
# Show the lines
#self.laneDetector.showImage()
# cv2.imshow('DetectedLines', detectedLanes.astype(float)*255)
# cv2.waitKey(15)
def getMaskAll(self):
return self.maskall
def cbImage(self, img_msg):
image = np.asarray(self.bridge.imgmsg_to_cv(img_msg))
self.images.append(image)
self.imageStamps.append(img_msg.header.stamp.to_sec())
class Ros2Python:
def __init__(self,
objectDetector=None,
laneDetector=None,
grassDetector=None):
# Subscribe to PointCloud2
self.subImage = rospy.Subscriber("/left/image_rect_color",\
Image, self.cbImage, queue_size=1)
self.subPoints = rospy.Subscriber("/points2",\
PointCloud2, self.cbPoints, queue_size=1)
self.images = []
self.mask = None
self.imageStamps = []
self.imageInd = 0
self.bridge = CvBridge()
self.showDetected = False
self.obstacleCloud = np.array([])
if objectDetector:
self.objectDetector = objectDetector
else:
self.objectDetector = ObstacleDetector((240, 320))
# We assume that the normal of the plane is pointing straight up
# in the vision (-Y direction)
self.objectDetector.setNormalConstraint(np.array([0., -1., -0.2]))
# And allow the search to deviate only 30 degrees from that
self.objectDetector.angleConstraint = 30.
if laneDetector:
self.laneDetector = laneDetector
else:
self.laneDetector = LaneDetector((240, 320))
if grassDetector:
self.grassDetector = grassDetector
else:
# Init with H,S,V thresholds
self.grassDetector = GrassDetector(45, 35, 35)
if VERBOSE:
print "Subscribed to /left/image_rect_color"
print "Subscribed to /points2"
def run(self):
rospy.init_node('Ros2Python', anonymous=True)
try:
rospy.spin()
except KeyboardInterrupt:
print "Shutting down liveObstacleDetection module"
def cbPoints(self, point_msg):
if VERBOSE:
print 'Received points. Size: (', point_msg.width, ',', point_msg.height, ')'
# Convert to numpy array (http://goo.gl/s6OGja)
cloud = np.array(points2cloud_to_array(point_msg))
# ObDetector only recalculates everything when the cloud is updated
cloud = np.dstack((cloud['x'], cloud['y'], cloud['z']))
pointT = point_msg.header.stamp.to_sec()
#print 'Point Time:', pointT
#print 'Image Times:', self.imageStamps
image = None
for i in range(len(self.imageStamps)):
imageT = self.imageStamps[i]
if imageT > pointT:
image = self.images[i]
break
if image == None:
image = self.images[i]
self.images = self.images[i:]
self.imageStamps = self.imageStamps[i:]
self.objectDetector.updateImage(image)
self.laneDetector.updateImage(image)
self.grassDetector.updateImage(image)
self.objectDetector.updatePoints(cloud)
##### Jared, these are the 2 most useful variables for you:
''' mask: is a numpy array the same size as the camera image. The
elements in the mask are set to 1 if that pixel is part of the
plane/ground, 2 if that pixel is an object, and 0 otherwise.
'''
self.mask = self.objectDetector.getPlaneObjectMask()
self.grassDetector.updateMask(self.mask == 1)
grassMask = np.logical_and(self.grassDetector.findGrass(),
self.mask != 2)
self.mask[grassMask] = 1
model = self.objectDetector.model
planeInd = model.coord2ind(np.array(np.where(self.mask == 1)).T)
planeInd = planeInd.astype(int)
success, coeff = model.optimiseModelCoefficients(\
planeInd,self.objectDetector.coeff)
success, newInd = model.selectWithinDistance(coeff, 0.15)
self.objectDetector.coeff = coeff
#print newInd.shape
#print newInd
self.mask[model.ind2coord(newInd)] = 1
''' obsacleCloud: a list of 2D points relative to the camera.
The positive X-axis goes to the right of the camera.
The positive Y-axis goes away from the camera.
TODO: Double check this =S
'''
self.obstacleCloud = self.objectDetector.getObstacleCloud()
# Do the lane detect also
detectedLanes = self.laneDetector.findLines()
self.detectedLanes = np.logical_and(detectedLanes, self.mask != 2)
self.laneDetector.showImage()
self.mask[self.detectedLanes] = 2
self.maskall = self.mask
if self.showDetected:
#print 'Showing Detected'
self.objectDetector.showDetected(self.mask)
self.showDetected = False
cv2.waitKey(10)
#self.objectDetector.showDepth()
# Show the plane and abjects
# im = self.objectDetector.image.copy()
# im[grassMask] = [255,0,0]
# im[mask==2] = [0,0,255]
# cv2.imshow('DetectedObjects', im)
# Show the lines
#self.laneDetector.showImage()
# cv2.imshow('DetectedLines', detectedLanes.astype(float)*255)
# cv2.waitKey(15)
def getMaskAll(self):
return self.maskall
def cbImage(self, img_msg):
image = np.asarray(self.bridge.imgmsg_to_cv(img_msg))
self.images.append(image)
self.imageStamps.append(img_msg.header.stamp.to_sec())
