def RadiationSimulator():
rospy.init_node('RadiationSimulator', anonymous=True)
#Publisher setup
DwellTimePub = []
DwellTimePub.append(rospy.Publisher('/gamma1', UInt32, queue_size=10))
DwellTimePub.append(rospy.Publisher('/gamma2', UInt32, queue_size=10))
DwellTimePub.append(rospy.Publisher('/gamma3', UInt32, queue_size=10))
myRad = UInt32(1)
#Listener setup
listener = tf.TransformListener()
sensorNames = ['/gamma1_tf', '/gamma2_tf', '/gamma3_tf']
sensorTimes = [time.time(), time.time(), time.time()]
#Radiation source setup
Radiation_Sources = []
Radiation_Sources.append(
RadiationSource.RadiationSource(2, [0, 0], background_flag=True))
Radiation_Sources.append(RadiationSource.RadiationSource(
500, [0.75, 0.75]))
#Control loop rate
main_rate = 10
rate = rospy.Rate(main_rate) # 10hz
while not rospy.is_shutdown():
#Simulate Radiation
#Update DwellTime_Map each time through the loop
for i in range(0, len(sensorNames)):
try:
(trans_gamma,
rot_gamma) = listener.lookupTransform('map', sensorNames[i],
rospy.Time(0))
#Update simulation data for each point
Sim_Rad = SimulationRadation([trans_gamma[0], trans_gamma[1]],
Radiation_Sources,
time.time() - sensorTimes[i])
sensorTimes[i] = time.time()
#Publish radiation
for rad_i in range(0, Sim_Rad):
DwellTimePub[i].publish(myRad)
time.sleep(0.0001)
#print("Gamma 1 CPS Updated")
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print(sensorNames[i] + " TF Exception in Radiation Simulator")
pass
CPS_Map = RadiationMap.Map()
CPS_GP_Map = RadiationMap.Map()
GP_Map = RadiationMap.GP_Map()
#Set up CPS maps for ground truth
DwellTime_Map_GT = RadiationMap.Map()
Count_Map_GT = RadiationMap.Map()
CPS_Map_GT = RadiationMap.Map()
delta_t = 0.1
#Define radation sources for simulation
Radiation_Simulation_Flag = True
Radiation_Sources = []
Radiation_Sources.append(
RadiationSource.RadiationSource(1, [0, 0], background_flag=True))
Radiation_Sources.append(RadiationSource.RadiationSource(2000, [1.5, 1.5]))
#Generate Plot
plt.ion()
fig, (ax1, ax2, ax3) = plt.subplots(1, 3)
im1 = ax1.imshow([[1, 2], [3, 4]],
interpolation='none',
origin='lower',
extent=[
DwellTime_Map.origin_x,
DwellTime_Map.origin_x + DwellTime_Map.width,
DwellTime_Map.origin_y,
DwellTime_Map.origin_y + DwellTime_Map.height
])
cbar1 = plt.colorbar(im1, ax=ax1)
GP_Map.origin_x,
GP_Map.origin_x + GP_Map.width,
GP_Map.origin_y,
GP_Map.origin_y + GP_Map.height
])
axarr[i, k].set_title(PlotTitles[i][k])
#locationMarker[i][k], = axarr[i,k].plot([1],[1],c=(255/255,20/255,147/255),marker=(3, 0, -90), markersize=10, linestyle='None')
plt.ion()
plt.show(block=False)
#Define radation sources for simulation
Radiation_Simulation_Flag = False
Radiation_Sources = []
Radiation_Sources.append(
RadiationSource.RadiationSource(1, [0, 0], background_flag=True))
#Radiation_Sources.append(RadiationSource.RadiationSource(500,[1,1]))
#Radiation_Sources.append(RadiationSource.RadiationSource(500,[-1,1]))
#Radiation_Sources.append(RadiationSource.RadiationSource(500,[0,0]))
Radiation_Sources.append(
RadiationSource.RadiationSource(
500, [np.random.rand() * 4 - 2,
np.random.rand() * 4 - 2]))
Radiation_Sources.append(
RadiationSource.RadiationSource(
500, [np.random.rand() * 4 - 2,
np.random.rand() * 4 - 2]))
Radiation_Sources.append(
RadiationSource.RadiationSource(
500, [np.random.rand() * 4 - 2,
np.random.rand() * 4 - 2]))
def MappingMain1():
global DwellTime_Map
global Count_Map
global Binary_Cost_Map
global Mask_Map
global trans
global listener
global RecordMapParameters
global GPMapParameters
global last_cost_update_time
#Decision-making parameters
threshold = 10
#Set up CPS maps
CPS_Map = RadiationMap.Map(*RecordMapParameters, dtype = 'float32')
GP_Map = RadiationMap.GP_Map(*RecordMapParameters)
Binary_Cost_Map = RadiationMap.Map(*RecordMapParameters, dtype = bool)
#Binary_Cost_Map.grid = np.full((384, 384), False, dtype = bool)
Mask_Map = RadiationMap.Map(*RecordMapParameters, dtype = bool)
Mask_Map.grid = np.full((384, 384), -1)
rospy.loginfo("Maps Created")
# Initialie node
rospy.init_node('MappingMain1', anonymous=True)
#Set up transform listener
listener = tf.TransformListener()
#Set up subscribers
rospy.Subscriber('/gamma1', UInt32, callback_gamma1)
rospy.Subscriber('/gamma2', UInt32, callback_gamma2)
rospy.Subscriber('/gamma3', UInt32, callback_gamma3)
rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid, callback_costmap)
rospy.Subscriber('/map', OccupancyGrid, callback_map)
#Define radation sources for simulation
Radiation_Simulation_Flag = False
Radiation_Sources = []
Radiation_Sources.append(RadiationSource.RadiationSource(2,[0,0],background_flag=True))
Radiation_Sources.append(RadiationSource.RadiationSource(500,[0,1]))
RadSimDelta_t1 = time.time()
RadSimDelta_t2 = time.time()
ros_rate = 10
last_gp_time = time.time()
last_plot_time = time.time()
last_target_time = time.time()
last_save_time = time.time()
last_cost_update_time = time.time()
last_dwell_time = time.time()
rate = rospy.Rate(ros_rate) # 10hz
while not rospy.is_shutdown():
#Get transform to baselink
try:
(trans, rot) = listener.lookupTransform('map', '/base_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
#print("trans" + repr(trans))
#Update DwellTime_Map each time through the loop
if Radiation_Simulation_Flag: #For simulation update with robot center
DwellTime_Map.set_cell(trans[0], trans[1], time.time() - last_dwell_time, addto=True)
else: #For actual runs use detector center
try:
(trans_gamma, rot_gamma) = listener.lookupTransform('map', '/gamma1_tf', rospy.Time(0))
DwellTime_Map.set_cell(trans_gamma[0], trans_gamma[1], time.time() - last_dwell_time, addto=True)
CPS = Count_Map.get_cell(trans_gamma[0], trans_gamma[1]) / DwellTime_Map.get_cell(trans_gamma[0], trans_gamma[1])
CPS_Map.set_cell(trans_gamma[0], trans_gamma[1], CPS, addto=False)
#print("Gamma 1 CPS Updated")
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print("Gamma1 TF Exception")
pass
try:
(trans_gamma, rot_gamma) = listener.lookupTransform('map', '/gamma2_tf', rospy.Time(0))
DwellTime_Map.set_cell(trans_gamma[0], trans_gamma[1], time.time() - last_dwell_time, addto=True)
CPS = Count_Map.get_cell(trans_gamma[0], trans_gamma[1]) / DwellTime_Map.get_cell(trans_gamma[0], trans_gamma[1])
CPS_Map.set_cell(trans_gamma[0], trans_gamma[1], CPS, addto=False)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print("Gamma2 TF Exception")
pass
try:
(trans_gamma, rot_gamma) = listener.lookupTransform('map', '/gamma3_tf', rospy.Time(0))
DwellTime_Map.set_cell(trans_gamma[0], trans_gamma[1], time.time() - last_dwell_time, addto=True)
CPS = Count_Map.get_cell(trans_gamma[0], trans_gamma[1]) / DwellTime_Map.get_cell(trans_gamma[0], trans_gamma[1])
CPS_Map.set_cell(trans_gamma[0], trans_gamma[1], CPS, addto=False)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print("Gamma3 TF Exception")
pass
last_dwell_time = time.time()
#Generate Random Radation
if Radiation_Simulation_Flag:
RadSimDelta_t2 = time.time()
Sim_Rad = SimulationRadation([trans[0],trans[1]], Radiation_Sources, RadSimDelta_t2 - RadSimDelta_t1)
RadSimDelta_t1 = RadSimDelta_t2
#Update Count_Map
Count_Map.set_cell(trans[0], trans[1], Sim_Rad, addto=True)
#Update CPS maps
try:
CPS = Count_Map.get_cell(trans[0], trans[1]) / DwellTime_Map.get_cell(trans[0], trans[1])
CPS_Map.set_cell(trans[0], trans[1], CPS, addto=False)
except:
print("Error Updating CPS Map")
pass
#DwellTime_Map.set_cell(trans[0], trans[1], 5, addto=True) #temp or loc
#print(time.time())
#ASYNCHRONOUS FUNCTIONS AND UPDATES
#Only update map at a given rate (hz)
gp_rate = .5;
if time.time() > last_gp_time + 1/gp_rate:
#Update CPS
#CPS_map.grid = Count_Map.grid / DwellTime_Map.grid
#Update GP
GP_Map.Calc_GP_Local(DwellTime_Map, CPS_Map, [trans[0], trans[1], np.degrees(rot)])
last_gp_time = time.time()
#print("Max Grid Value: " + repr(CPS_Map.grid.max()))
#Mask_Map.plot_map()
plot_rate = .5;
if time.time() > last_plot_time + 1/plot_rate:
#Update CPS
#CPS_map.grid = Count_Map.grid / DwellTime_Map.grid
#print("Max Grid Value: " + repr(CPS_Map.grid.max()))
DwellTime_Map.plot_map()
CPS_Map.plot_map()
GP_Map.plot_map()
#Mask_Map.plot_map()
last_plot_time = time.time()
#Choose target and publish
target_rate = .5;
if time.time() > last_target_time + 1/target_rate:
[targetPosition, Q_orig, Q_binary] = TargetSelection([trans[0], trans[1], np.degrees(rot)], GP_Map, Mask_Map, threshold)
last_target_time = time.time()
#Publish target commands
#Save data for later plotting
save_rate = .5;
if time.time() > last_save_time + 1/save_rate:
#Save all desired maps
#Save Data
np.save('Map_Data/GP_Map_' + repr(time.time()), GP_Map.grid)
np.save('Map_Data/CPS_Map_' + repr(time.time()), CPS_Map.grid)
np.save('Map_Data/Mask_Map_' + repr(time.time()), Mask_Map.grid)
last_save_time = time.time()
#### not working workaround...
# map_rate = .5
# if time.time() > last_cost_update_time + 1/map_rate:
# # Update Mask Map
# if subscribed:
# if received:
# sub_cm.unregister()
# subscribed = False
# received = False
# else:
# sub_cm = rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid, callback_costmap)
# last_cost_update_time = time.time()
rate.sleep()
def MappingMain1():
global DwellTime_Map
global Count_Map
global trans
global listener
#Set up CPS maps
CPS_Map = RadiationMap.Map()
CPS_GP_Map = RadiationMap.Map()
GP_Map = RadiationMap.GP_Map()
rospy.loginfo("Maps Created")
# Initialie node
rospy.init_node('MappingMain1', anonymous=True)
#Set up transform listener
listener = tf.TransformListener()
#Set up subscribers
rospy.Subscriber('/gamma1', UInt32, callback_gamma1)
rospy.Subscriber('/gamma2', UInt32, callback_gamma2)
rospy.Subscriber('/gamma3', UInt32, callback_gamma3)
#Define radation sources for simulation
Radiation_Simulation_Flag = False
Radiation_Sources = []
Radiation_Sources.append(RadiationSource.RadiationSource(2,[0,0],background_flag=True))
Radiation_Sources.append(RadiationSource.RadiationSource(500,[0,1]))
RadSimDelta_t1 = time.time()
RadSimDelta_t2 = time.time()
ros_rate = 10
last_plot_time = time.time()
last_dwell_time = time.time()
rate = rospy.Rate(ros_rate) # 10hz
while not rospy.is_shutdown():
#Get transform to baselink
try:
(trans, rot) = listener.lookupTransform('map', '/base_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
#print("trans" + repr(trans))
#Update DwellTime_Map each time through the loop
#print(time.time() - last_dwell_time)
if Radiation_Simulation_Flag: #For simulation update with robot center
DwellTime_Map.set_cell(trans[0], trans[1], time.time() - last_dwell_time, addto=True)
else: #For actual runs use detector center
try:
(trans_gamma, rot_gamma) = listener.lookupTransform('map', '/gamma1_tf', rospy.Time(0))
DwellTime_Map.set_cell(trans_gamma[0], trans_gamma[1], time.time() - last_dwell_time, addto=True)
CPS = Count_Map.get_cell(trans_gamma[0], trans_gamma[1]) / DwellTime_Map.get_cell(trans_gamma[0], trans_gamma[1])
CPS_Map.set_cell(trans_gamma[0], trans_gamma[1], CPS, addto=False)
#print("Gamma 1 CPS Updated")
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print("Gamma1 TF Exception")
pass
try:
(trans_gamma, rot_gamma) = listener.lookupTransform('map', '/gamma2_tf', rospy.Time(0))
DwellTime_Map.set_cell(trans_gamma[0], trans_gamma[1], time.time() - last_dwell_time, addto=True)
CPS = Count_Map.get_cell(trans_gamma[0], trans_gamma[1]) / DwellTime_Map.get_cell(trans_gamma[0], trans_gamma[1])
CPS_Map.set_cell(trans_gamma[0], trans_gamma[1], CPS, addto=False)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print("Gamma2 TF Exception")
pass
try:
(trans_gamma, rot_gamma) = listener.lookupTransform('map', '/gamma3_tf', rospy.Time(0))
DwellTime_Map.set_cell(trans_gamma[0], trans_gamma[1], time.time() - last_dwell_time, addto=True)
CPS = Count_Map.get_cell(trans_gamma[0], trans_gamma[1]) / DwellTime_Map.get_cell(trans_gamma[0], trans_gamma[1])
CPS_Map.set_cell(trans_gamma[0], trans_gamma[1], CPS, addto=False)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print("Gamma3 TF Exception")
pass
last_dwell_time = time.time()
#Generate Random Radation
if Radiation_Simulation_Flag:
RadSimDelta_t2 = time.time()
Sim_Rad = SimulationRadation([trans[0],trans[1]], Radiation_Sources, RadSimDelta_t2 - RadSimDelta_t1)
RadSimDelta_t1 = RadSimDelta_t2
#Update Count_Map
Count_Map.set_cell(trans[0], trans[1], Sim_Rad, addto=True)
#Update CPS maps
try:
CPS = Count_Map.get_cell(trans[0], trans[1]) / DwellTime_Map.get_cell(trans[0], trans[1])
CPS_Map.set_cell(trans[0], trans[1], CPS, addto=False)
except:
print("Error Updating CPS Map")
pass
#DwellTime_Map.set_cell(trans[0], trans[1], 5, addto=True) #temp or loc
#print(time.time())
#Only update map at a given rate (hz)
map_rate = .5;
if time.time() > last_plot_time + 1/map_rate:
#Update CPS
#CPS_map.grid = Count_Map.grid / DwellTime_Map.grid
#Update GP
GP_Map.Calc_GP(DwellTime_Map, CPS_Map)
last_plot_time = time.time()
#print("Max Grid Value: " + repr(CPS_Map.grid.max()))
DwellTime_Map.plot_map()
CPS_Map.plot_map()
GP_Map.plot_map()
#Save Data
np.save('Map_Data/GP_Map_' + repr(time.time()), GP_Map.grid)
np.save('Map_Data/CPS_Map_' + repr(time.time()), CPS_Map.grid)
rate.sleep()
def MappingMain1():
global DwellTime_Map
global Count_Map
global trans
# Initialie node
rospy.init_node('MappingMain1', anonymous=True)
#Set up subscribers
rospy.Subscriber('/gamma1', String, callback)
#Set up transform listener
listener = tf.TransformListener()
#Define radation sources for simulation
Radiation_Simulation_Flag = True
Radiation_Sources = []
Radiation_Sources.append(
RadiationSource.RadiationSource(2, [0, 0], background_flag=True))
Radiation_Sources.append(RadiationSource.RadiationSource(500, [1.5, 1.5]))
RadSimDelta_t1 = time.time()
RadSimDelta_t2 = time.time()
#Set up CPS maps
CPS_Map = RadiationMap.Map()
CPS_GP_Map = RadiationMap.Map()
ros_rate = 10
last_plot_time = time.time()
rate = rospy.Rate(ros_rate) # 10hz
while not rospy.is_shutdown():
#Get transform to baselink
try:
(trans, rot) = listener.lookupTransform('map', '/base_link',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
#print("trans" + repr(trans))
#Update DwellTime_Map each time through the loop
DwellTime_Map.set_cell(trans[0],
trans[1],
1.0 / float(ros_rate),
addto=True)
#Generate Random Radation
if Radiation_Simulation_Flag:
RadSimDelta_t2 = time.time()
Sim_Rad = SimulationRadation([trans[0], trans[1]],
Radiation_Sources,
RadSimDelta_t2 - RadSimDelta_t1)
RadSimDelta_t1 = RadSimDelta_t2
#Update Count_Map
Count_Map.set_cell(trans[0], trans[1], Sim_Rad, addto=True)
#Update CPS maps
CPS = Count_Map.get_cell(trans[0], trans[1]) / DwellTime_Map.get_cell(
trans[0], trans[1])
CPS_Map.set_cell(trans[0], trans[1], CPS, addto=False)
#DwellTime_Map.set_cell(trans[0], trans[1], 5, addto=True) #temp or loc
#print(time.time())
#Only update map at a given rate (hz)
map_rate = 1
if time.time() > last_plot_time + 1 / map_rate:
last_plot_time = time.time()
print("Max Grid Value: " + repr(CPS_Map.grid.max()))
DwellTime_Map.plot_map()
CPS_Map.plot_map()
rate.sleep()
#Create buffer so it doesn't sample off edge of map
# for i in range(0,len(PlotList)):
# for k in range(0,len(PlotList[i])):
# im[i][k] = axarr[i,k].imshow(PlotList[i][k], interpolation='none', origin = 'lower', extent = [GP_Map.origin_x, GP_Map.origin_x+GP_Map.width, GP_Map.origin_y, GP_Map.origin_y+GP_Map.height])
# axarr[i,k].set_title(PlotTitles[i][k])
# #locationMarker[i][k], = axarr[i,k].plot([1],[1],c=(255/255,20/255,147/255),marker=(3, 0, -90), markersize=10, linestyle='None')
plt.ion()
plt.show(block=False)
#Define radation sources for simulation
Radiation_Simulation_Flag = False
Radiation_Sources = []
Radiation_Sources.append(
RadiationSource.RadiationSource(.5, [0, 0], background_flag=True))
#Radiation_Sources.append(RadiationSource.RadiationSource(500,[1,1]))
#Radiation_Sources.append(RadiationSource.RadiationSource(500,[-1,1]))
#Radiation_Sources.append(RadiationSource.RadiationSource(500,[0,0]))
Radiation_Sources.append(
RadiationSource.RadiationSource(50, [
np.random.rand() * MapWidth - MapWidth / 2,
np.random.rand() * MapWidth - MapWidth / 2
]))
Radiation_Sources.append(
RadiationSource.RadiationSource(50, [
np.random.rand() * MapWidth - MapWidth / 2,
np.random.rand() * MapWidth - MapWidth / 2
]))
Radiation_Sources.append(
RadiationSource.RadiationSource(50, [