print(
"(defaults: echo_input=1, startpt_offset=13, RLcall='ReflexiveLayer_v3.0.py', HLcall='HabitualLayer_v3.0.py', DLcall='pseudoDL_v3.0.py', connection='ws://localhost:9090/')"
)
print(" ")
print("Starting subcall_estRLHLDL.py...")
try:
print("sys.argv = " + str(sys.argv))
# python subcall_est_RLHLDL.py echo_input Estcall RLcall HLcall DLcall connection ...
argvdefaults = [
1, "pseudoEstimator_v3.0.py", "ReflexiveLayer_v3.0.py",
"HabitualLayer_v3.0.py", "pseudoDL_v3.0.py", "ws://localhost:9090/"
] # default connection valid because starting rosbridge from this file :)
[echo_input, Estcall, RLcall, HLcall, DLcall,
connection] = fF.readArgvpieces(sys.argv, argvdefaults)
echo_input = int(echo_input) # in case overwrote int with string
#startpt_offset = int(startpt_offset) # in case overwrote int with string
#
# reset terminal string for new/next terminal run(s):
#
tabtitle = []
command = []
tabgeometry = []
tabtitle.extend(['State Estimator'])
command.extend([
''' cd estimator
python %(Estcall)s %(echo_input)d %(connection)s
''' % locals()
def __init__(self,argvdefaults,argv): # __init__(self,arg):
#super(DeliberativeLayer,self).__init__(arg) # super inheritance
[echo_input,
self.connection] = fF.readArgvpieces(argv,argvdefaults)
echo_input = int(echo_input) # in case overwrote int with string
print("Force wait until rosbridge interface and parameter server come online...")
testReturn = wsF.generalRecvParamServer(self.connection,'/',['rosdistro'],'block',1.0) # wait 1.0sec before recheck
print("Parameter server is up and responding!")
#[robotType,simType] = wsF.generalRecvParamServer(connection,'/globals/',['robotType','simType'],'block')
#[stepTime] = constRuntimeParamsDL(robotType,simType)
Flag_true_from_some_flag_dict = {1: 'yes', 0: None}
[self.some_flag] = wsF.generalRecvParamServer(self.connection,"/globals/",['some_flag'],'block')
self.Flag_true = Flag_true_from_some_flag_dict[self.some_flag] # initialize; note that Flag_true = None should stay constant for some_flag=0
[self.waypointFile,self.waypointsOnly] = menuDL(echo_input)
[self.growlen,[self.timetry,self.retries],
[self.derr,self.mult],self.passthrough,
self.solverUse,self.mapIsRasterNotPolygons,self.plannerstr,
[self.intmap,self.rParams,self.rover],
[self.polytope_list,self.polyunits,self.polydim],
self.OMPLsolverData,
syspathstrToAppend] = sC.setupSolverParams(connection=self.connection,globallayertopic="/globals/deliberative/",raster_params=None) # raster_params=[[rwidth,rheight,thresh,self.growlen],mapname,[self.timetry,self.retries]]
sys.path.append(syspathstrToAppend)
self.someCoeff = 20
self.relaxBy = 2
self.t = 0; self.t0 = None
self.loc = [0]*3 ; self.loc0 = [None]*3 # [x,y,z] ; [x0,y0,z0]
self.ang = [0]*3 ; self.ang0 = [None]*3 # [r,p,h] ; [r0,p0,h0]
self.quat = [1,0,0,0] ; self.quat0 = [None]*4 # [qx,qy,qz,qw] ; [qx0,qy0,qz0,qw0]
self.new_cur_pos = None
#self.simSteps = 1;
#stepTime defined above
#self.tinc = stepTime
self.seq=0
self.current_waypoints = []
#self.current_waypoints = None
self.new_current_waypoints = []
self.risk = []
self.riskHold = []
self.pts_calcd = 1 # for saving path-calculation runs
self.path_list = []
self.pathpoint_on = 0
#self.opstatus = None # force it to start as not defined
self.waypoint_on = -1 # force it to get waypoints at beginning if needed
self.hlstatus = None #; self.checkthis = True # force it to start sending waypoints at beginning if from file, HL should already be waiting (w/o initial HL status send)
self.hlwaypoints = None
self.hlseq = None
self.hlwayseq = None
self.some_constraint_request = None
self.time_request = None
self.actuator_request = None
self.stateDL = 'start'; self.opmsgreceived = False; self.hlmsgreceived = False
self.senddlstatusWhere = {100: 'senddlstatusToHL', # we are good so far
200: 'senddlstatusToOp', # we found a problem that requires a fail-up
900: 'senddlstatusToOp'} # we found a problem that requires a fail-up
self.dlstatus = 100 # start assuming things are okay :)
self.handling = None
self.handlingHold = None
try:
print("sys.argv = " + str(sys.argv))
# python subcall_est_RLHLDL.py echo_input Estcall RLcall HLcall DLcall connection ...
argvdefaults = [1,
"pseudoEstimator_v3.0.py",
"ReflexiveLayer_v3.0.py",
"HabitualLayer_v3.0.py",
"pseudoDL_v3.0.py",
"ws://localhost:9090/"] # default connection valid because starting rosbridge from this file :)
[echo_input,
Estcall,
RLcall,
HLcall,
DLcall,
connection] = fF.readArgvpieces(sys.argv,argvdefaults)
echo_input = int(echo_input) # in case overwrote int with string
#startpt_offset = int(startpt_offset) # in case overwrote int with string
#
# reset terminal string for new/next terminal run(s):
#
tabtitle = []
command = []
tabgeometry = []
tabtitle.extend(['State Estimator'])
command.extend([''' cd estimator
python %(Estcall)s %(echo_input)d %(connection)s
''' % locals()])
tabgeometry.extend(['80x24+0+500']) # COLUMNSxROWS+X+Y
def __init__(self, argvdefaults, argv): # __init__(self,arg):
#super(DeliberativeLayer,self).__init__(arg) # super inheritance
[echo_input, self.connection] = fF.readArgvpieces(argv, argvdefaults)
echo_input = int(echo_input) # in case overwrote int with string
print(
"Force wait until rosbridge interface and parameter server come online..."
)
testReturn = wsF.generalRecvParamServer(
self.connection, '/', ['rosdistro'], 'block',
1.0) # wait 1.0sec before recheck
print("Parameter server is up and responding!")
#[robotType,simType] = wsF.generalRecvParamServer(connection,'/globals/',['robotType','simType'],'block')
#[stepTime] = constRuntimeParamsDL(robotType,simType)
Flag_true_from_some_flag_dict = {1: 'yes', 0: None}
[self.some_flag] = wsF.generalRecvParamServer(self.connection,
"/globals/",
['some_flag'], 'block')
self.Flag_true = Flag_true_from_some_flag_dict[
self.
some_flag] # initialize; note that Flag_true = None should stay constant for some_flag=0
[self.waypointFile, self.waypointsOnly] = menuDL(echo_input)
[
self.growlen, [self.timetry, self.retries], [self.derr, self.mult],
self.passthrough, self.solverUse, self.mapIsRasterNotPolygons,
self.plannerstr, [self.intmap, self.rParams, self.rover],
[self.polytope_list, self.polyunits,
self.polydim], self.OMPLsolverData, syspathstrToAppend
] = sC.setupSolverParams(
connection=self.connection,
globallayertopic="/globals/deliberative/",
raster_params=None
) # raster_params=[[rwidth,rheight,thresh,self.growlen],mapname,[self.timetry,self.retries]]
sys.path.append(syspathstrToAppend)
self.someCoeff = 20
self.relaxBy = 2
self.t = 0
self.t0 = None
self.loc = [0] * 3
self.loc0 = [None] * 3 # [x,y,z] ; [x0,y0,z0]
self.ang = [0] * 3
self.ang0 = [None] * 3 # [r,p,h] ; [r0,p0,h0]
self.quat = [1, 0, 0, 0]
self.quat0 = [None] * 4 # [qx,qy,qz,qw] ; [qx0,qy0,qz0,qw0]
self.new_cur_pos = None
#self.simSteps = 1;
#stepTime defined above
#self.tinc = stepTime
self.seq = 0
self.current_waypoints = []
#self.current_waypoints = None
self.new_current_waypoints = []
self.risk = []
self.riskHold = []
self.pts_calcd = 1 # for saving path-calculation runs
self.path_list = []
self.pathpoint_on = 0
#self.opstatus = None # force it to start as not defined
self.waypoint_on = -1 # force it to get waypoints at beginning if needed
self.hlstatus = None #; self.checkthis = True # force it to start sending waypoints at beginning if from file, HL should already be waiting (w/o initial HL status send)
self.hlwaypoints = None
self.hlseq = None
self.hlwayseq = None
self.some_constraint_request = None
self.time_request = None
self.actuator_request = None
self.stateDL = 'start'
self.opmsgreceived = False
self.hlmsgreceived = False
self.senddlstatusWhere = {
100: 'senddlstatusToHL', # we are good so far
200:
'senddlstatusToOp', # we found a problem that requires a fail-up
900: 'senddlstatusToOp'
} # we found a problem that requires a fail-up
self.dlstatus = 100 # start assuming things are okay :)
self.handling = None
self.handlingHold = None