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