rot_scen.py

   1 # -*- coding: utf-8 -*-
   2 """Rotate scenario."""
   3 from json import dumps
   4 from math import cos, pi, sin
   5 from plot import load_scenario
   6
   7 ROTI = 11
   8 RA = ROTI*pi/8
   9 TOLOAD = "rpar"
  10 SAVEAS = "rotlpar"
  11
  12 if __name__ == "__main__":
  13     s = load_scenario("{}.json".format(TOLOAD))
  14     ns = {}
  15     # init
  16     (x, y, h) = s["init"]
  17     nx = x * cos(RA) - y * sin(RA)
  18     ny = x * sin(RA) + y * cos(RA)
  19     nh = h + RA
  20     ns["init"] = (nx, ny, nh)
  21     # goal
  22     (x, y, h) = s["goal"]
  23     nx = x * cos(RA) - y * sin(RA)
  24     ny = x * sin(RA) + y * cos(RA)
  25     nh = h + RA
  26     ns["goal"] = (nx, ny, nh)
  27     # slot
  28     ns["slot"] = {}
  29     ns["slot"]["polygon"] = []
  30     for (x, y) in s["slot"]["polygon"]:
  31         nx = x * cos(RA) - y * sin(RA)
  32         ny = x * sin(RA) + y * cos(RA)
  33         ns["slot"]["polygon"].append((nx, ny))
  34     #obst
  35     ns["obst"] = []
  36     for o in s["obst"]:
  37         try:
  38             no = {}
  39             no["segment"] = []
  40             for (x, y) in o["segment"]:
  41                 nx = x * cos(RA) - y * sin(RA)
  42                 ny = x * sin(RA) + y * cos(RA)
  43                 no["segment"].append((nx, ny))
  44             ns["obst"].append(no)
  45         except:
  46             pass
  47     # save
  48     with open("{}.json".format(SAVEAS), "w") as f:
  49         f.write(dumps(ns))