def add_map_table(name, wi):
table = rospy.get_param('wg-sushi-tables/'+name)
upper_left = Point(x = table['upper_left']['x'], y = table['upper_left']['y'],
z = table['upper_left']['z'])
lower_left = Point(x = table['lower_left']['x'], y = table['lower_left']['y'],
z = table['lower_left']['z'])
upper_right = Point(x = table['upper_right']['x'], y = table['upper_right']['y'],
z = table['upper_right']['z'])
lower_right = Point(x = table['lower_right']['x'], y = table['lower_right']['y'],
z = table['lower_right']['z'])
table_object = CollisionObject()
table_object.header.frame_id = wi.world_frame
table_object.id = name
table_pose = Pose()
table_pose.position.x = (upper_left.x+upper_right.x+lower_left.x+lower_right.x)/4.0
table_pose.position.y = (upper_left.y+upper_right.y+lower_left.y+lower_right.y)/4.0
table_pose.position.z = (upper_left.z+upper_right.z+lower_left.z+lower_right.z)/4.0
table_pose.orientation.w = 1.0
table_shape = Shape()
table_shape.type = Shape.MESH
table_shape.vertices = [gt.inverse_transform_point(lower_left, table_pose),
gt.inverse_transform_point(upper_left, table_pose),
gt.inverse_transform_point(upper_right, table_pose),
gt.inverse_transform_point(lower_right, table_pose)]
table_shape.triangles = [0, 1, 2, 2, 3, 0]
table_object.shapes.append(table_shape)
table_upper_pose = copy.deepcopy(table_pose)
table_upper_pose.position.z += 0.02
table_object.poses.append(table_pose)
table_object.poses.append(table_upper_pose)
table_object.shapes.append(table_shape)
table_lower_pose = copy.deepcopy(table_pose)
table_lower_pose.position.z -= 0.02
table_object.poses.append(table_lower_pose)
table_object.shapes.append(table_shape)
if name == 'dirty_table':
#kind of a hack =D
for i in range(5):
table_lower_pose = copy.deepcopy(table_lower_pose)
table_lower_pose.position.z -= 0.02
table_object.poses.append(table_lower_pose)
table_object.shapes.append(table_shape)
wi.add_object(table_object)
table = Table()
table.pose.header = table_object.header
table.pose.pose = table_object.poses[0]
table.convex_hull = table_object.shapes[0]
return table
if point_in_triangle(p, v_i, v_j, v_k):
return True
return False
if __name__ == '__main__':
import roslib; roslib.load_manifest('simple_utils')
from matplotlib import pyplot as plt
from geometry_msgs.msg import Point
from arm_navigation_msgs.msg import Shape
table = Shape()
table.vertices = [
Point(0.0, 0.0, 0.0),
Point(0.5, 1.0, 0.0),
Point(0.7, 1.2, 0.0),
Point(1.5, 0.2, 0.0)
]
table.triangles = [0, 1, 2, 0, 2, 3]
on_table = []
not_on_table = []
for ii in range(300):
p = np.random.random((2,))
point = Point(p[0], p[1], 0.0)
if point_on_table(point, table):
on_table.append(p)
else:
not_on_table.append(p)
vertices = [np.array((v.x, v.y)) for v in table.vertices]
def add_tables(self):
if self.screenshot:
return ['', '', '']
table = CollisionObject()
table.header.frame_id = self.wi.world_frame
table.operation.operation = table.operation.ADD
shape = Shape()
shape.type = shape.MESH
#center table
shape.vertices = [
Point(x=-0.2, y=-0.4, z=CENTER_TABLE_HEIGHT),
Point(x=0.97, y=-0.6, z=CENTER_TABLE_HEIGHT),
Point(x=1.0, y=0.25, z=CENTER_TABLE_HEIGHT),
Point(x=-0.25, y=0.3, z=CENTER_TABLE_HEIGHT)
]
shape.triangles = [0, 1, 2, 2, 3, 0]
pose = Pose()
pose.orientation.w = 1.0
poseb = copy.deepcopy(pose)
poseb.position.z = -0.02
poset = copy.deepcopy(pose)
poset.position.z = 0.02
table.shapes.append(shape)
table.shapes.append(shape)
table.shapes.append(shape)
table.poses.append(poset)
table.poses.append(pose)
table.poses.append(poseb)
table.id = 'center_table'
self.wi.add_object(table)
#table near the door
table.id = 'door_table'
shape.vertices = [
Point(x=-2.4, y=1.84, z=DOOR_TABLE_HEIGHT),
Point(x=-1.15, y=1.75, z=DOOR_TABLE_HEIGHT),
Point(x=-1.15, y=2.5, z=DOOR_TABLE_HEIGHT),
Point(x=-2.4, y=2.5, z=DOOR_TABLE_HEIGHT)
]
self.wi.add_object(table)
#table in far corner
table.id = 'far_corner'
shape.vertices = [
Point(x=3, y=-2.7, z=FAR_TABLE_HEIGHT),
Point(x=2.4, y=-3.8, z=FAR_TABLE_HEIGHT),
Point(x=3.2, y=-4.3, z=FAR_TABLE_HEIGHT),
Point(x=3.8, y=-3.2, z=FAR_TABLE_HEIGHT)
]
self.wi.add_object(table)
if self.fake_walls:
#these are the table feet
foot = CollisionObject()
foot.header.frame_id = self.wi.world_frame
foot.operation.operation = foot.operation.ADD
foot.id = "far_corner_foot"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = [0.1, 0.5, FAR_TABLE_HEIGHT / 2.0]
pose = Pose()
pose.position.x = 3
pose.position.y = -3.4
pose.position.z = shape.dimensions[2] / 2.0
angle = 0.5
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
foot.shapes.append(shape)
foot.poses.append(pose)
self.wi.add_object(foot)
foot.id = "center_table_foot1"
shape.dimensions = [0.1, 0.75, 0.3]
pose.position.x = 0.9
pose.position.y = -0.1
pose.position.z = shape.dimensions[2] / 2.0
angle = 0
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
self.wi.add_object(foot)
foot.id = "center_table_foot2"
pose.position.x = -0.2
self.wi.add_object(foot)
foot.id = "door_table_foot"
pose.position.x = -1.25
pose.position.y = 2.1
self.wi.add_object(foot)
return ['center_table', 'door_table', 'far_corner']
def get_obj():
attached_obj = AttachedCollisionObject()
obj = CollisionObject()
shape = Shape()
pose = Pose()
vert = Point()
verts = []
in_verts = True
in_position = True
f = open('test.txt', 'r')
for line in f:
fields = line.split(':')
fields = [fields[i].strip() for i in range(len(fields))]
# print fields
if fields[0] == "frame_id":
obj.header.frame_id = fields[1]
elif fields[0] == "id":
obj.id = fields[1]
elif fields[0] == "operation" and fields[1] != "":
obj.operation.operation = int(fields[1])
elif fields[0] == "type":
shape.type = int(fields[1])
elif fields[0] == "triangles":
array = fields[1][1:-2]
ind = array.split(',')
inds = [int(i) for i in ind]
shape.triangles = inds
elif fields[0] == "x" and in_verts:
vert = Point()
vert.x = float(fields[1])
elif fields[0] == "y" and in_verts:
vert.y = float(fields[1])
elif fields[0] == "z" and in_verts:
vert.z = float(fields[1])
verts.append(vert)
elif fields[0] == "poses":
in_verts = False
elif fields[0] == "x" and in_position:
pose.position.x = float(fields[1])
elif fields[0] == "y" and in_position:
pose.position.y = float(fields[1])
elif fields[0] == "z" and in_position:
pose.position.z = float(fields[1])
in_position = False
elif fields[0] == "x" and not in_position:
pose.orientation.x = float(fields[1])
elif fields[0] == "y" and not in_position:
pose.orientation.y = float(fields[1])
elif fields[0] == "z" and not in_position:
pose.orientation.z = float(fields[1])
elif fields[0] == "w":
pose.orientation.w = float(fields[1])
obj.id = "graspable_object_1001"
shape.vertices = verts
obj.shapes = [shape]
obj.poses = [pose]
attached_obj.object = obj
return attached_obj
