diff --git a/example_two_car_lane_switch.py b/example_two_car_lane_switch.py
index c4ca8ca4a254c6c1676d31a0278118b1a98cec98..bd8f4798a1e57b00365033daff6d1abcf15958cd 100644
--- a/example_two_car_lane_switch.py
+++ b/example_two_car_lane_switch.py
@@ -46,7 +46,6 @@ def controller(ego:State, other:State, lane_map):
from ourtool.agents.car_agent import CarAgent
from ourtool.scenario.scenario import Scenario
-# from user.simple_sensor import SimpleSensor
from user.simple_map import SimpleMap, SimpleMap2
import matplotlib.pyplot as plt
import numpy as np
@@ -60,11 +59,10 @@ if __name__ == "__main__":
car = CarAgent('car2', file_name=input_code_name)
scenario.add_agent(car)
scenario.add_map(SimpleMap2())
- # scenario.set_sensor(SimpleSensor())
scenario.set_init(
- [[10,0,0,0.5], [0,-3,0,1.0]],
+ [[0,-3,0,1.0], [10,-3,0,0.5]],
[
- (VehicleMode.Normal, LaneMode.Lane1),
+ (VehicleMode.Normal, LaneMode.Lane2),
(VehicleMode.Normal, LaneMode.Lane2)
]
)
diff --git a/ourtool/automaton/guard.py b/ourtool/automaton/guard.py
index a0957c9e294d5da1526e46c979009c065e0b01a3..f011ae0f70686863e243ad58fc8b661fec4a30c8 100644
--- a/ourtool/automaton/guard.py
+++ b/ourtool/automaton/guard.py
@@ -1,7 +1,10 @@
import re
from typing import List, Dict
-from ourtool.automaton.hybrid_io_automaton import HybridIoAutomaton
-from pythonparser import Guard
+import pickle
+# from ourtool.automaton.hybrid_io_automaton import HybridIoAutomaton
+# from pythonparser import Guard
+import ast
+import astunparse
class LogicTreeNode:
def __init__(self, data, child = [], val = None, mode_guard = None):
@@ -10,6 +13,7 @@ class LogicTreeNode:
self.val = val
self.mode_guard = mode_guard
+'''
class GuardExpression:
def __init__(self, root:LogicTreeNode=None, logic_str:str=None, guard_list=None):
self._func_dict = {}
@@ -340,8 +344,109 @@ class GuardExpression:
root.mode_guard = root.child[0].mode_guard
root.child = root.child[0].child
return True
+'''
+
+class GuardExpressionAst:
+ def __init__(self, guard_list):
+ self.ast_list = []
+ for guard in guard_list:
+ self.ast_list.append(guard.ast)
+
+ def evaluate_guard(self, agent, continuous_variable_dict, discrete_variable_dict, lane_map):
+ res = True
+ for node in self.ast_list:
+ tmp = self._evaluate_guard(node, agent, continuous_variable_dict, discrete_variable_dict, lane_map)
+ res = tmp and res
+ if not res:
+ break
+ return res
+
+ def _evaluate_guard(self, root, agent, cnts_var_dict, disc_var_dict, lane_map):
+ if isinstance(root, ast.Compare):
+ left = self._evaluate_guard(root.left, agent, cnts_var_dict, disc_var_dict, lane_map)
+ right = self._evaluate_guard(root.comparators[0], agent, cnts_var_dict, disc_var_dict, lane_map)
+ if isinstance(root.ops[0], ast.GtE):
+ return left>=right
+ elif isinstance(root.ops[0], ast.Gt):
+ return left>right
+ elif isinstance(root.ops[0], ast.Lt):
+ return left<right
+ elif isinstance(root.ops[0], ast.LtE):
+ return left<=right
+ elif isinstance(root.ops[0], ast.Eq):
+ return left == right
+ elif isinstance(root.ops[0], ast.NotEq):
+ return left != right
+ else:
+ raise ValueError(f'Node type {root} from {astunparse.unparse(root)} is not supported')
+
+ elif isinstance(root, ast.BoolOp):
+ if isinstance(root.op, ast.And):
+ res = True
+ for val in root.values:
+ tmp = self._evaluate_guard(val, agent, cnts_var_dict, disc_var_dict, lane_map)
+ res = res and tmp
+ if not res:
+ break
+ return res
+ elif isinstance(root.op, ast.Or):
+ res = False
+ for val in root.values:
+ tmp = self._evaluate_guard(val, agent, cnts_var_dict, disc_var_dict, lane_map)
+ res = res or tmp
+ if res:
+ break
+ return res
+ elif isinstance(root, ast.BinOp):
+ left = self._evaluate_guard(root.left, agent, cnts_var_dict, disc_var_dict, lane_map)
+ right = self._evaluate_guard(root.right, agent, cnts_var_dict, disc_var_dict, lane_map)
+ if isinstance(root.op, ast.Sub):
+ return left - right
+ elif isinstance(root.op, ast.Add):
+ return left + right
+ else:
+ raise ValueError(f'Node type {root} from {astunparse.unparse(root)} is not supported')
+ elif isinstance(root, ast.Call):
+ expr = astunparse.unparse(root)
+ # Check if the root is a function
+ if 'map' in expr:
+ # tmp = re.split('\(|\)',expr)
+ # while "" in tmp:
+ # tmp.remove("")
+ # for arg in tmp[1:]:
+ # if arg in disc_var_dict:
+ # expr = expr.replace(arg,f'"{disc_var_dict[arg]}"')
+ # res = eval(expr)
+ for arg in disc_var_dict:
+ expr = expr.replace(arg, f'"{disc_var_dict[arg]}"')
+ for arg in cnts_var_dict:
+ expr = expr.replace(arg, str(cnts_var_dict[arg]))
+ res = eval(expr)
+ return res
+ elif isinstance(root, ast.Attribute):
+ expr = astunparse.unparse(root)
+ expr = expr.strip('\n')
+ if expr in disc_var_dict:
+ val = disc_var_dict[expr]
+ for mode_name in agent.controller.modes:
+ if val in agent.controller.modes[mode_name]:
+ val = mode_name+'.'+val
+ break
+ return val
+ elif expr in cnts_var_dict:
+ val = cnts_var_dict[expr]
+ return val
+ elif root.value.id in agent.controller.modes:
+ return expr
+ elif isinstance(root, ast.Constant):
+ return root.value
+ else:
+ raise ValueError(f'Node type {root} from {astunparse.unparse(root)} is not supported')
if __name__ == "__main__":
- tmp = GuardExpression()
- tmp.construct_tree_from_str('(other_x-ego_x<20) and other_x-ego_x>10 and other_vehicle_lane==ego_vehicle_lane')
+ with open('tmp.pickle','rb') as f:
+ guard_list = pickle.load(f)
+ tmp = GuardExpressionAst(guard_list)
+ # tmp.evaluate_guard()
+ # tmp.construct_tree_from_str('(other_x-ego_x<20) and other_x-ego_x>10 and other_vehicle_lane==ego_vehicle_lane')
print("stop")
\ No newline at end of file
diff --git a/ourtool/map/lane_map.py b/ourtool/map/lane_map.py
index fb536600f0551d75fee7a3e5d563ad72d5341bac..c62e2f48993e6e478dd6cb888b1879e15ecf8c8f 100644
--- a/ourtool/map/lane_map.py
+++ b/ourtool/map/lane_map.py
@@ -6,8 +6,8 @@ from ourtool.map.lane_segment import LaneSegment
class LaneMap:
def __init__(self, lane_seg_list:List[LaneSegment] = []):
self.lane_segment_dict:Dict[str, LaneSegment] = {}
- self.left_lane_dict:Dict[str, LaneSegment] = {}
- self.right_lane_dict:Dict[str, LaneSegment] = {}
+ self.left_lane_dict:Dict[str, List[str]] = {}
+ self.right_lane_dict:Dict[str, List[str]] = {}
for lane_seg in lane_seg_list:
self.lane_segment_dict[lane_seg.id] = lane_seg
self.left_lane_dict[lane_seg.id] = []
diff --git a/ourtool/scenario/scenario.py b/ourtool/scenario/scenario.py
index c2e232f05848a1e5f660e1050ead82ef03ad97f3..e5cbe70efc5372021b55d08b4f22733fa5e16e0a 100644
--- a/ourtool/scenario/scenario.py
+++ b/ourtool/scenario/scenario.py
@@ -1,9 +1,10 @@
from typing import Dict, List
import copy
import itertools
+import ast
from ourtool.agents.base_agent import BaseAgent
-from ourtool.automaton.guard import GuardExpression
+from ourtool.automaton.guard import GuardExpressionAst
from pythonparser import Guard
from pythonparser import Reset
from ourtool.simulator.simulator import Simulator
@@ -101,10 +102,11 @@ class Scenario:
guard_list.append(item)
elif isinstance(item, Reset):
reset_list.append(item.code)
- guard_expression = GuardExpression(guard_list=guard_list)
-
+ # guard_expression = GuardExpression(guard_list=guard_list)
+ guard_expression = GuardExpressionAst(guard_list)
# Map the values to variables using sensor
continuous_variable_dict, discrete_variable_dict = self.sensor.sense(self, agent, state_dict, self.map)
+
'''Execute functions related to map to see if the guard can be satisfied'''
'''Check guards related to modes to see if the guards can be satisfied'''
'''Actually plug in the values to see if the guards can be satisfied'''