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 GenPlaneFromWorldPoints(self, point1, point2, point3):
AB = [(point2.x-point1.x),(point2.y-point1.y),(point2.z-point1.z)]
AC = [(point3.x-point1.x),(point3.y-point1.y),(point3.z-point1.z)]
cProd = cross(AB,AC)
newPlane = Plane(cProd[0],cProd[1],cProd[2],0)
# d is the value we expect to get when we multiply the normal
# times any point on the plane
normal = cProd
newPlane.d = -dot(normal, [point1.x,point1.y,point1.z])
return newPlane
# DEAD CODE
if otherPt > 0:
return 1
else:
otherPt = -1*point1.x*newPlane[0] + -1*point1.y*newPlane[1] + -1*point1.z*newPlane[2] + newPlane.d + normal
if otherPt > 0:
return 0
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()
