OBST_COUNT = 8
-def gen_init():
- """Generate car init position."""
- # TODO if changed, change ``gen_slot`` accordingly
- return (0, 0, 0)
-
-
def gen_slot_at(x, y, h, parallel=True):
"""Generate slot at specified coordinates."""
length = 5.3
]
-def gen_slot(l=5.3, w=2.4):
- """Generate parking slot."""
- parallel = True if random() < 0.5 else False
- if parallel and l == 5.3 and w == 2.4:
- l = 6.5
- w = 2.2
- elif parallel:
- ol = l
- l = w
- w = ol
- right = 1.0 if random() < 0.5 else -1.0
- r = SLOT_RADI
- angl = uniform(0, 2 * pi)
- x = r * cos(angl)
- y = r * sin(angl)
- h = uniform(0, 2 * pi)
- if parallel:
- return [
- [
- x + l/2 * cos(h - pi/2 * right),
- y + l/2 * sin(h - pi/2 * right),
- ],
- [
- x + w * cos(h) + l/2 * cos(h - pi/2 * right),
- y + w * sin(h) + l/2 * sin(h - pi/2 * right),
- ],
- [
- x + w * cos(h) + l/2 * cos(h + pi/2 * right),
- y + w * sin(h) + l/2 * sin(h + pi/2 * right),
- ],
- [
- x + l/2 * cos(h + pi/2 * right),
- y + l/2 * sin(h + pi/2 * right),
- ],
- ]
- else:
- return [
- [
- x + w/2 * cos(h - pi/2 * right),
- y + w/2 * sin(h - pi/2 * right),
- ],
- [
- x + l * cos(h) + w/2 * cos(h - pi/2 * right),
- y + l * sin(h) + w/2 * sin(h - pi/2 * right),
- ],
- [
- x + l * cos(h) + w/2 * cos(h + pi/2 * right),
- y + l * sin(h) + w/2 * sin(h + pi/2 * right),
- ],
- [
- x + w/2 * cos(h + pi/2 * right),
- y + w/2 * sin(h + pi/2 * right),
- ],
- ]
-
-
-def gen_obst_at(x, y, h):
+def gen_obst_at(x, y, h, length=0.5, width=0.5):
"""Generate obstacle at specific coordinates."""
- length = 0.5
- width = 0.5
return [
[
x + width/2 * cos(h - pi/2) + length/2 * cos(h),
]]
-def gen_obst():
- """Generate obstacles array."""
+def random_init():
+ """Generate car init pose.
+
+ It should be random, but now it is just [0, 0, 0].
+ """
+ return [0, 0, 0]
+
+
+def random_slot():
+ """Generate random parking slot."""
+ parallel = True if random() < 0.5 else False
+ r = SLOT_RADI
+ angl = uniform(0, 2 * pi)
+ x = r * cos(angl)
+ y = r * sin(angl)
+ h = uniform(0, 2 * pi)
+ return gen_slot_at(x, y, h, parallel)
+
+
+def random_obstacles():
+ """Generate a list of random obstacles."""
obstacles = []
min_r = ((W/2)**2 + ((WB+L)/2)**2)**0.5 + (OBST_W**2 + OBST_L**2)**0.5 / 2
for i in range(OBST_COUNT):
- l = OBST_L
- w = OBST_W
angl = uniform(0, 2 * pi)
r = uniform(min_r**2, (SLOT_RADI - 5)**2)**0.5
x = r * cos(angl)
y = r * sin(angl)
h = uniform(0, 2 * pi)
- obstacles.append([
- [
- x + w/2 * cos(h - pi/2) + l/2 * cos(h),
- y + w/2 * sin(h - pi/2) + l/2 * sin(h),
- ],
- [
- x + w/2 * cos(h - pi/2) - l/2 * cos(h),
- y + w/2 * sin(h - pi/2) - l/2 * sin(h),
- ],
- [
- x + w/2 * cos(h + pi/2) - l/2 * cos(h),
- y + w/2 * sin(h + pi/2) - l/2 * sin(h),
- ],
- [
- x + w/2 * cos(h + pi/2) + l/2 * cos(h),
- y + w/2 * sin(h + pi/2) + l/2 * sin(h),
- ],
- [
- x + w/2 * cos(h - pi/2) + l/2 * cos(h),
- y + w/2 * sin(h - pi/2) + l/2 * sin(h),
- ],
- ])
+ obstacles.append(gen_obst_at(x, y, h, OBST_L, OBST_W))
return obstacles
SCENARIOS = {
+ "pe05": {
+ "init": [0, 0, 0],
+ "slot": gen_slot_at(10, 0, 0, False),
+ "obst": [gen_obst_at(*p) for p in [
+ [8, 2.5, 0], [6, 2.5, 0],
+ [10.25, 1.5, 0], [10.25, -1.5, 0]]]},
+ "pe04": {
+ "init": [0, 0, 0],
+ "slot": gen_slot_at(10, 0, 0, False),
+ "obst": [gen_obst_at(*p) for p in [
+ [10.25, 1.5, 0], [10.25, -1.5, 0]]]},
+ "pe03": {
+ "init": [0, 0, 0],
+ "slot": gen_slot_at(15, 0, pi/2, False),
+ "obst": [gen_obst_at(*p) for p in [
+ [13.5, 0.25, 0], [16.5, 0.25, 0],
+ [12.5, -2.5, 0], [17.5, -2.5, 0]]]},
+ "pe02": {
+ "init": [0, 0, 0],
+ "slot": gen_slot_at(15, 0, pi/2, False),
+ "obst": [gen_obst_at(*p) for p in [
+ [13.5, 0.25, 0], [16.5, 0.25, 0],
+ [13.5, -5, 0], [16.5, -5, 0]]]},
+ "pe01": {
+ "init": [0, 0, 0],
+ "slot": gen_slot_at(15, 0, pi/2, False),
+ "obst": [gen_obst_at(*p) for p in [
+ [13.5, 0.25, 0], [16.5, 0.25, 0],
+ [15, -5, 0]]]},
+ "pa05": {
+ "init": [0, 0, 0],
+ "slot": gen_slot_at(12, 2, -pi/2, True),
+ "obst": [gen_obst_at(*p) for p in [
+ [7, 0, 0], [7, 2, 0], [7, 4, 0], [7, -2, 0],
+ [9, -2, 0], [11, -2, 0]]]},
+ "pa04": {
+ "init": [0, 0, 0],
+ "slot": gen_slot_at(12, -2, -pi/2, True),
+ "obst": [gen_obst_at(*p) for p in [
+ [7, -2, 0], [7, -4, 0], [7, -6, 0], [9, -6, 0], [11, -6, 0],
+ [7, -2, 0], [11, 0, 0], [15, 0, 0], [19, 0, 0],
+ [7, 3, 0], [11, 5, 0], [15, 3, 0], [19, 3, 0]]]},
+ "pa03": {
+ "init": [0, 0, 0],
+ "slot": gen_slot_at(15, 0, 0, True),
+ "obst": [gen_obst_at(*p) for p in [
+ [15.25, -3.5, 0], [15.25, 3.5, 0],
+ [7.5, 2.5, 0], [10, -2.5, 0], [12.5, 0, 0]]]},
+ "pa02": {
+ "init": [0, 0, 0],
+ "slot": gen_slot_at(15, 0, 0, True),
+ "obst": [gen_obst_at(*p) for p in [
+ [15.25, -3.5, 0], [15.25, 3.5, 0],
+ [10, 5, 0], [10, 0, 0], [12.5, 0, 0]]]},
"pa01": {
- "init": gen_init(),
+ "init": [0, 0, 0],
"slot": gen_slot_at(15, 0, 0, True),
- "obst": [gen_obst_at(p) for p in [[], [], []]]}}
+ "obst": [gen_obst_at(*p) for p in [
+ [15.25, -3.5, 0], [15.25, 3.5, 0],
+ [12.5, -2.5, 0], [10, 0, 0], [7.5, 2.5, 0]]]}}
if __name__ == "__main__":
sc = ""
if len(sys.argv) == 2:
sc = sys.argv[1]
if sc in SCENARIOS:
- print(dumps(SCENARIOS()[sc]))
+ print(dumps(SCENARIOS[sc]))
else:
print(dumps({
- "init": gen_init(),
- "slot": gen_slot(),
- "obst": gen_obst()}))
+ "init": random_init(),
+ "slot": random_slot(),
+ "obst": random_obstacles()}))
projects / hubacji1 / iamcar2.git / blobdiff