211 lines
4.3 KiB
Python
211 lines
4.3 KiB
Python
from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion)
|
|
# ##DELETE THIS
|
|
# class Position():
|
|
# def __init__(self):
|
|
# self.x = 0
|
|
# self.y = 0
|
|
# self.z = 0
|
|
|
|
# class Orientation():
|
|
# def __init__(self):
|
|
# self.x = 0
|
|
# self.y = 0
|
|
# self.z = 0
|
|
# self.w = 0
|
|
|
|
# class EAngle():
|
|
# def __init__(self):
|
|
# self.roll = 0
|
|
# self.pitch = 0
|
|
# self.yaw = 0
|
|
|
|
# class Pose():
|
|
# def __init__(self):
|
|
# self.position = Position()
|
|
# self.orientation = Orientation()
|
|
# ##DELETE THIS
|
|
|
|
startv = Pose()
|
|
startv.position.x = 0.474
|
|
startv.position.y = 0.759
|
|
startv.position.z = 0.234
|
|
startv.orientation.x = -0.7071067811865476
|
|
startv.orientation.y = -0.7071067811865475
|
|
startv.orientation.z = 4.329780281177466e-17
|
|
startv.orientation.w = 4.329780281177467e-17
|
|
|
|
starth = Pose()
|
|
starth.position.x = 0.464
|
|
starth.position.y = 0.804
|
|
starth.position.z = 0.126
|
|
starth.orientation.x = 00.7071067811865476
|
|
starth.orientation.y = 0.7071067811865475
|
|
starth.orientation.z = 4.329780281177466e-17
|
|
starth.orientation.w = 4.329780281177467e-17
|
|
|
|
brick1 = Pose()
|
|
brick1.position.x = 0.615
|
|
brick1.position.y = 0.194
|
|
brick1.position.z = -0.131
|
|
brick1.orientation.x = -0.7071067811865476
|
|
brick1.orientation.y = -0.7071067811865475
|
|
brick1.orientation.z = 4.329780281177466e-17
|
|
brick1.orientation.w = 4.329780281177467e-17
|
|
|
|
brick2 = Pose()
|
|
brick2.position.x = 0.615
|
|
brick2.position.y = 0.346
|
|
brick2.position.z = -0.125
|
|
brick2.orientation.x = -0.7071067811865476
|
|
brick2.orientation.y = -0.7071067811865475
|
|
brick2.orientation.z = 4.329780281177466e-17
|
|
brick2.orientation.w = 4.329780281177467e-17
|
|
|
|
brick3 = Pose()
|
|
brick3.position.x = 0.615
|
|
brick3.position.y = 0.532
|
|
brick3.position.z = -0.122
|
|
brick3.orientation.x = -0.7071067811865476
|
|
brick3.orientation.y = -0.7071067811865475
|
|
brick3.orientation.z = 4.329780281177466e-17
|
|
brick3.orientation.w = 4.329780281177467e-17
|
|
|
|
brick4 = Pose()
|
|
brick4.position.x = 0.615
|
|
brick4.position.y = 0.240
|
|
brick4.position.z = -0.0499
|
|
brick4.orientation.x = -0.7071067811865476
|
|
brick4.orientation.y = -0.7071067811865475
|
|
brick4.orientation.z = 4.329780281177466e-17
|
|
brick4.orientation.w = 4.329780281177467e-17
|
|
|
|
brick5 = Pose()
|
|
brick5.position.x = 0.615
|
|
brick5.position.y = 0.449
|
|
brick5.position.z = -0.058
|
|
brick5.orientation.x = -0.7071067811865476
|
|
brick5.orientation.y = -0.7071067811865475
|
|
brick5.orientation.z = 4.329780281177466e-17
|
|
brick5.orientation.w = 4.329780281177467e-17
|
|
|
|
brick6 = Pose()
|
|
brick6.position.x = 0.615
|
|
brick6.position.y = 0.247
|
|
brick6.position.z = 0.123
|
|
brick6.orientation.x = -0.7071067811865476
|
|
brick6.orientation.y = -0.7071067811865475
|
|
brick6.orientation.z = 4.329780281177466e-17
|
|
brick6.orientation.w = 4.329780281177467e-17
|
|
|
|
brick7 = Pose()
|
|
brick7.position.x = 0.615
|
|
brick7.position.y = 0.441
|
|
brick7.position.z = 0.138
|
|
brick7.orientation.x = -0.7071067811865476
|
|
brick7.orientation.y = -0.7071067811865475
|
|
brick7.orientation.z = 4.329780281177466e-17
|
|
brick7.orientation.w = 4.329780281177467e-17
|
|
|
|
brick8 = Pose()
|
|
brick8.position.x = 0.635
|
|
brick8.position.y = 0.334
|
|
brick8.position.z = 0.209
|
|
brick8.orientation.x = -0.7071067811865476
|
|
brick8.orientation.y = -0.7071067811865475
|
|
brick8.orientation.z = 4.329780281177466e-17
|
|
brick8.orientation.w = 4.329780281177467e-17
|
|
|
|
brick9 = Pose()
|
|
brick9.position.x = 0.635
|
|
brick9.position.y = 0.349
|
|
brick9.position.z = 0.406
|
|
brick9.orientation.x = -0.7071067811865476
|
|
brick9.orientation.y = -0.7071067811865475
|
|
brick9.orientation.z = 4.329780281177466e-17
|
|
brick9.orientation.w = 4.329780281177467e-17
|
|
|
|
|
|
brick_directions_notf = [
|
|
{
|
|
'id':'tv',
|
|
'pose':startv,
|
|
'isVertical':True
|
|
},
|
|
{'id':'th',
|
|
'pose':starth,
|
|
'isVertical':False
|
|
},
|
|
{'id':'b1',
|
|
'pose':brick1,
|
|
'isVertical':True
|
|
},
|
|
{'id':'b2',
|
|
'pose':brick2,
|
|
'isVertical':True
|
|
},
|
|
{'id':'b3',
|
|
'pose':brick3,
|
|
'isVertical':True
|
|
},
|
|
{'id':'b4',
|
|
'pose':brick4,
|
|
'isVertical':False
|
|
},
|
|
{'id':'b5',
|
|
'pose':brick5,
|
|
'isVertical':False
|
|
},
|
|
{'id':'b6',
|
|
'pose':brick6,
|
|
'isVertical':True
|
|
},
|
|
{'id':'b7',
|
|
'pose':brick7,
|
|
'isVertical':True
|
|
},
|
|
{'id':'b8',
|
|
'pose':brick8,
|
|
'isVertical':False
|
|
},
|
|
{'id':'b9',
|
|
'pose':brick9,
|
|
'isVertical':True
|
|
}]
|
|
|
|
bdf = brick_directions_notf
|
|
|
|
tower_instructions = [ bdf[0],
|
|
bdf[2],
|
|
bdf[0],
|
|
bdf[3],
|
|
bdf[0],
|
|
bdf[4],
|
|
bdf[1],
|
|
bdf[5],
|
|
bdf[1],
|
|
bdf[6],
|
|
bdf[0],
|
|
bdf[7],
|
|
bdf[0],
|
|
bdf[8],
|
|
bdf[1],
|
|
bdf[9],
|
|
bdf[0],
|
|
bdf[10],
|
|
]
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|