def PublishSpace(self, bb, sensor,algorithm, aux_payload):
planes = self.GenPlanes(bb)
if len(planes) == 0:
print "WARNING: Camera2BoundingBox:PublishSpace(): Returning None because len(planes) == 0"
return None
space = Convex_Space()
space.planes = planes
space.sensor = sensor
space.algorithm = algorithm
space.aux_payload = aux_payload
self.publisher.publish(space)
# some callers might be interested in seeing what I just published
return space
def init():
output_name = "intersector_input"
publisher = rospy.Publisher(output_name, Convex_Space)
rospy.init_node('intersector')
print "Sleep for 2 seconds to allow the 3d_intersector time to initialize..."
rospy.Rate(.5).sleep()
print "Sleep done..."
while True:
# this is a horizontal slicer
space = Convex_Space()
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = 1
plane.d = -1
space.planes.append(plane);
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = -1
plane.d = 0
space.planes.append(plane);
space.sensor = "horizontal_slicer"
space.algorithm = "horizontal_slicer"
space.aux_payload = "slicer:horizontal"
publisher.publish(space)
rospy.Rate(1).sleep()
def extractSpace(self):
# this extracts a Convex_Space object from this intersection object.
retval = Convex_Space()
retval.planes = self.planes # TODO: cull planes, someday
retval.sensor = "intersector"
retval.algorithm = "intersector"
# the aux_payload of the new Convex_Space is the union of all the
# input payloads, plus the point list and bounding box we have
# calculated.
retval.aux_payload = ""
for space in self.convex_spaces:
# print "AUX PAYLOAD: " + space.aux_payload
retval.aux_payload += space.aux_payload + "\n"
# print
# print retval.aux_payload
# print
# this little loop lists the points we have calculated...
min_x = max_x = self.points[0].x
min_y = max_y = self.points[0].y
min_z = max_z = self.points[0].z
pts_payload = ""
global marker # added by Jeremy
for pt in self.points:
pts_payload += " (%f,%f,%f)" % (pt.x,pt.y,pt.z)
marker.points.append(Point(pt.x,pt.y,pt.z)) # added by Jeremy
if pt.x < min_x:
min_x = pt.x
if pt.x > max_x:
max_x = pt.x
if pt.y < min_y:
min_y = pt.y
if pt.y > max_y:
max_y = pt.y
if pt.z < min_z:
min_z = pt.z
if pt.z > max_z:
max_z = pt.z
bb_payload = "x=(%f,%f) y=(%f,%f) z=(%f,%f)" % (min_x,max_x, min_y,max_y, min_z,max_z)
""" # added by Jeremy - show only bounding box
marker.points.append(Point(min_x,min_y,min_z))
marker.points.append(Point(min_x,min_y,max_z))
marker.points.append(Point(min_x,max_y,max_z))
marker.points.append(Point(max_x,max_y,max_z))
marker.points.append(Point(max_x,max_y,min_z))
marker.points.append(Point(max_x,min_y,min_z))
marker.points.append(Point(max_x,min_y,max_z))
marker.points.append(Point(min_x,max_y,min_z))
"""
retval.aux_payload += "convex_set: %s\n" % pts_payload
retval.aux_payload += "bounding_box: %s\n" % bb_payload
# also, add a count of the input spaces as a quick estimate of how
# interesting this object is.
retval.aux_payload += "convex_space_count: %d" % len(self.convex_spaces)
return retval
def init():
output_name = "intersector_input"
publisher = rospy.Publisher(output_name, Convex_Space)
rospy.init_node('intersector')
print "Sleep for 2 seconds to allow the 3d_intersector time to initialize..."
rospy.Rate(.5).sleep()
print "Sleep done..."
print "Sending recorded shape information..."
# the following was recorded from "rostopic echo /intersector_input" during a run of one of our Gazebo tests
# ---
# planes: [
# a: 0.0
# b: 0.0
# c: 1.0
# d: -1.0,
# a: 0.0
# b: 0.0
# c: -1.0
# d: 0.0]
# sensor: horizontal_slicer
# algorithm: horizontal_slicer
# aux_payload: slicer:horizontal
# ---
# planes: [
# a: -0.0507812052965
# b: 0.0
# c: -0.0324326232076
# d: -0.0972978696227,
# a: 3.45435147153e-08
# b: 0.048828125
# c: -0.0260353088379
# d: -0.0781059265137,
# a: 0.0507812090218
# b: 0.0
# c: 0.0299530699849
# d: 0.0898592099547,
# a: -3.12536556635e-08
# b: -0.048828125
# c: 0.0235557556152
# d: 0.0706672668457]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:blue
# ---
# planes: [
# a: -5.19797671572e-09
# b: -0.025390625
# c: 0.0039176940918
# d: 0.0117530822754,
# a: -0.0253906045109
# b: 0.0
# c: -0.0159187652171
# d: -0.0477562956512,
# a: 6.05333960735e-09
# b: 0.025390625
# c: -0.00456237792969
# d: -0.0136871337891,
# a: 0.0253906045109
# b: 0.0
# c: 0.0152740813792
# d: 0.0458222441375]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:white
# ---
# planes: [
# a: 6.38232577899e-09
# b: -0.025390625
# c: -0.00481033325195
# d: -0.0144309997559,
# a: -0.0253906026483
# b: 0.0
# c: -0.0162163116038
# d: -0.0486489348114,
# a: -5.52696244327e-09
# b: 0.025390625
# c: 0.00416564941406
# d: 0.0124969482422,
# a: 0.0253906045109
# b: 0.0
# c: 0.0155716277659
# d: 0.0467148832977]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:green
# ---
# planes: [
# a: 3.46700481657e-08
# b: -0.048828125
# c: -0.0261306762695
# d: -0.0783920288086,
# a: -0.0507812052965
# b: 0.0
# c: -0.0324326232076
# d: -0.0972978696227,
# a: -3.13801891139e-08
# b: 0.048828125
# c: 0.0236511230469
# d: 0.0709533691406,
# a: 0.0507812090218
# b: 0.0
# c: 0.0299530699849
# d: 0.0898592099547]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:red
# ---
space = Convex_Space()
space.sensor = "horizontal_slicer"
space.algorithm = "horizontal_slicer"
space.aux_payload = "slicer:horizontal"
space.planes = []
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = 1
plane.d = -1
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = -1
plane.d = 0
space.planes.append(plane)
# print "PUBLISHING: %s" % space
publisher.publish(space)
# ----
space = Convex_Space()
space.sensor = "/overhead_cam"
space.algorithm = "ian3"
space.aux_payload = "ian3:blue"
space.planes = []
plane = Plane()
plane.a = -0.0507812052965
plane.b = 0.0
plane.c = -0.0324326232076
plane.d = -0.0972978696227
space.planes.append(plane)
plane = Plane()
plane.a = 3.45435147153e-08
plane.b = 0.048828125
plane.c = -0.0260353088379
plane.d = -0.0781059265137
space.planes.append(plane)
plane = Plane()
plane.a = 0.0507812090218
plane.b = 0.0
plane.c = 0.0299530699849
plane.d = 0.0898592099547
space.planes.append(plane)
plane = Plane()
plane.a = -3.12536556635e-08
plane.b = -0.048828125
plane.c = 0.0235557556152
plane.d = 0.0706672668457
space.planes.append(plane)
# print "PUBLISHING: %s" % space
publisher.publish(space)
# ----
space = Convex_Space()
space.sensor = "/overhead_cam"
space.algorithm = "ian3"
space.aux_payload = "ian3:white"
space.planes = []
plane = Plane()
plane.a = -5.19797671572e-09
plane.b = -0.025390625
plane.c = 0.0039176940918
plane.d = 0.0117530822754
space.planes.append(plane)
plane = Plane()
plane.a = -0.0253906045109
plane.b = 0.0
plane.c = -0.0159187652171
plane.d = -0.0477562956512
space.planes.append(plane)
plane = Plane()
plane.a = 6.05333960735e-09
plane.b = 0.025390625
plane.c = -0.00456237792969
plane.d = -0.0136871337891
space.planes.append(plane)
plane = Plane()
plane.a = 0.0253906045109
plane.b = 0.0
plane.c = 0.0152740813792
plane.d = 0.0458222441375
space.planes.append(plane)
# print "PUBLISHING: %s" % space
publisher.publish(space)
# ----
space = Convex_Space()
space.sensor = "/overhead_cam"
space.algorithm = "ian3"
space.aux_payload = "ian3:green"
space.planes = []
plane = Plane()
plane.a = 6.38232577899e-09
plane.b = -0.025390625
plane.c = -0.00481033325195
plane.d = -0.0144309997559
space.planes.append(plane)
plane = Plane()
plane.a = -0.0253906026483
plane.b = 0.0
plane.c = -0.0162163116038
plane.d = -0.0486489348114
space.planes.append(plane)
plane = Plane()
plane.a = -5.52696244327e-09
plane.b = 0.025390625
plane.c = 0.00416564941406
plane.d = 0.0124969482422
space.planes.append(plane)
plane = Plane()
plane.a = 0.0253906045109
plane.b = 0.0
plane.c = 0.0155716277659
plane.d = 0.0467148832977
space.planes.append(plane)
# print "PUBLISHING: %s" % space
publisher.publish(space)
# ----
space = Convex_Space()
space.sensor = "/overhead_cam"
space.algorithm = "ian3"
space.aux_payload = "ian3:red"
space.planes = []
plane = Plane()
plane.a = 3.46700481657e-08
plane.b = -0.048828125
plane.c = -0.0261306762695
plane.d = -0.0783920288086
space.planes.append(plane)
plane = Plane()
plane.a = -0.0507812052965
plane.b = 0.0
plane.c = -0.0324326232076
plane.d = -0.0972978696227
space.planes.append(plane)
plane = Plane()
plane.a = -3.13801891139e-08
plane.b = 0.048828125
plane.c = 0.0236511230469
plane.d = 0.0709533691406
space.planes.append(plane)
plane = Plane()
plane.a = 0.0507812090218
plane.b = 0.0
plane.c = 0.0299530699849
plane.d = 0.0898592099547
space.planes.append(plane)
# print "PUBLISHING: %s" % space
publisher.publish(space)
# END OF RECORDED PLANES
return
# this is a horizontal slicer
space = Convex_Space()
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = 1
plane.d = -1
space.planes.append(plane);
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = -1
plane.d = 0
space.planes.append(plane);
space.sensor = "horizontal_slicer"
space.algorithm = "horizontal_slicer"
space.aux_payload = "slicer:horizontal"
publisher.publish(space)
print "Just published a horizontal_slicer"
rospy.Rate(1).sleep()
# this generates something approximating the sort of pyramid which
# camera2boundingBox would produce.
#
# Characteristic points:
# Camera:
# (0, 0, 3)
# Planes:
# (3, 3, 0)
# (3, 6, 0)
# (6, 3, 0)
# (6, 6, 0)
space = Convex_Space()
plane = Plane()
plane.a = -1
plane.b = 0
plane.c = -1
plane.d = 3
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = -1
plane.c = -1
plane.d = 3
space.planes.append(plane)
plane = Plane()
plane.a = 0.5
plane.b = 0
plane.c = 1
plane.d = -3
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = 0.5
plane.c = 1
plane.d = -3
space.planes.append(plane)
space.sensor = "pseudo_pyramid"
space.algorithm = "pseudo_pyramid"
space.aux_payload = ""
publisher.publish(space)
print "Just published a pseudo-pyramid"
rospy.Rate(1).sleep()
# test a few passes where we gradually reduce the box size.
for radius in inclusive_range(10*1000, 5*1000, -1000):
space = Convex_Space()
plane = Plane()
plane.a = 1
plane.b = 0
plane.c = 0
plane.d = -radius
print "plane: %s" % plane
space.planes.append(plane)
plane = Plane()
plane.a = -1
plane.b = 0
plane.c = 0
plane.d = -radius
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = 1
plane.c = 0
plane.d = -radius
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = -1
plane.c = 0
plane.d = -radius
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = 1
plane.d = -radius
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = -1
plane.d = -radius
space.planes.append(plane)
space.sensor = "plane_slicer"
space.algorithm = "plane_slicer"
space.aux_payload = "plane_slicer_radius=%d" % radius
publisher.publish(space)
print "Just published a plane-slicing space with radius=%d" % radius
rospy.Rate(1).sleep()
# remember that the initial box, defined in 3d_intersector, is +- 10*1000 in each dimension.
#
# With radius == 10*1000, we get 3 key points, which are the exact centers of 3 different
# faces; the plane slices through those. However, it works out that the plane also slices
# through the 3 nearby corners of the original box.
#
# With radius == 20*1000, we get 3 key points, but these are now midpoints of edges, instead
# of the center of faces. Thus, each plane just slices off one corner of the cube.
#
# We'll test with 20k, then 10k, and see how our intersector handles it.
for radius in inclusive_range(20*1000, 10*1000, -10*1000):
space = Convex_Space()
for a in range(-1, 1+1, 2):
for b in range(-1, 1+1, 2):
for c in range(-1, 1+1, 2):
plane = Plane()
plane.a = a
plane.b = b
plane.c = c
plane.d = -radius # need negative d value, since we are adding it to the left-hand side of the inequality, and then checking if the value is small enough.
space.planes.append(plane)
space.sensor = "corner_slicer"
space.algorithm = "corner_slicer:radius=%d" % radius
space.aux_payload = "corner_slicer_radius=%d" % radius
publisher.publish(space)
print "Just published a corner-slicing space with radius=%d" % radius
rospy.Rate(1).sleep()
