t = 0
currentPosition = [
np.random.rand() * 3.8 - 1.9,
np.random.rand() * 3.8 - 1.9,
np.random.rand() * 360
]
#currentPosition = [-1.9,-1.9,0]
targetPosition = currentPosition
Sim_Rad = [0, 0, 0]
robotRadius = 0.1 #robot radius in meters
threshold = 10
frameCnt = 0
timer = [0] * 10
#Maps
DwellTime_Map = RadiationMap.Map()
Count_Map = RadiationMap.Map()
Mask_Map = RadiationMap.Map()
CPS_Map = RadiationMap.Map()
CPS_GP_Map = RadiationMap.Map()
GP_Map = RadiationMap.GP_Map()
print("Maps Created")
Q_binary = GP_Map.grid
Q_orig = GP_Map.grid
Mask_Map.grid += 1
for i in range(0, len(Mask_Map.grid)):
Mask_Map.grid[i][0] = 0
Mask_Map.grid[i][len(Mask_Map.grid) - 1] = 0
Mask_Map.grid[0][i] = 0
Mask_Map.grid[len(Mask_Map.grid) - 1][i] = 0
import RadiationSource
def SimulationRadation(current_location, sources, delta_t, rand_flag=True):
rad = 0
for source in sources:
rad += source.GenerateRandomRadiation(current_location,
delta_t,
rand_flag=rand_flag)
return rad
#Set up CPS maps
DwellTime_Map = RadiationMap.Map()
Count_Map = RadiationMap.Map()
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 = []
global trans
global Binary_Cost_Map
global Mask_Map
global RecordMapParameters
global GPMapParameters
global last_cost_update_time
global Cost_Map
#Map parameters
MapWidth = 10.0001
MaskMapWidth = 19.200001
RecordMapParameters = [-MapWidth / 2, -MapWidth / 2, 0.05, MapWidth, MapWidth]
GPMapParameters = [-MapWidth / 2, -MapWidth / 2, 0.05, MapWidth, MapWidth]
MaskMapParameters = [-10, -10, 0.05, MaskMapWidth, MaskMapWidth]
DwellTime_Map = RadiationMap.Map(*RecordMapParameters, dtype='float32')
Count_Map = RadiationMap.Map(*RecordMapParameters, dtype='int16')
#Initial Time
last_cost_update_time = time.time()
#---------------CallBacks------------------
def callback_gamma1(data):
global Count_Map
global listener
#listener_gamma = tf.TransformListener()
#Get transform to baselink
try:
(trans_gamma,
def MappingMain1():
global DwellTime_Map
global CPS_Map
global Count_Map
global Binary_Cost_Map
global Mask_Map
global Cost_Map
global Cost_Samples_Map
global trans
global listener
global RecordMapParameters
global GPMapParameters
global occupancy_Map
global last_cost_update_time
#Decision-making parameters
Qthreshold = 1.6
targetPosition = [0, 0, 0]
#Set up CPS maps
CPS_Map = RadiationMap.Map(*RecordMapParameters, dtype='float32')
GP_Map = RadiationMap.GP_Map(*RecordMapParameters)
Binary_Cost_Map = RadiationMap.Map(*MaskMapParameters, dtype=bool)
Q_Map = RadiationMap.Map(*RecordMapParameters)
Q2_Map = RadiationMap.Map(*RecordMapParameters)
#Binary_Cost_Map.grid = np.full((384, 384), False, dtype = bool)
Mask_Map = RadiationMap.Map(*MaskMapParameters, dtype=bool)
occupancy_Map = RadiationMap.Map(*MaskMapParameters, dtype=bool)
Cost_Map = RadiationMap.Map(*MaskMapParameters, dtype='int32')
Cost_Samples_Map = RadiationMap.Map(*MaskMapParameters, dtype='int32')
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('/bloodhound/dwelltimeupdate', Point,
callback_dwelltimeupdate)
rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid,
callback_costmap)
rospy.Subscriber('/map', OccupancyGrid, callback_map)
#Setup publisher
GoalSimpletoPublish = PoseStamped()
QuaternionPublisher = Quaternion()
GoalSimplePublisher = rospy.Publisher('/move_base_simple/goal',
PoseStamped,
queue_size=10)
GoalPublisher = rospy.Publisher('/move_base/goal',
MoveBaseActionGoal,
queue_size=10)
GoalSimpletoPublish.header.frame_id = "map"
GoaltoPublish = MoveBaseActionGoal()
GoaltoPublish.goal.target_pose.header.frame_id = "map"
ros_rate = 10
last_gp_time = time.time()
last_plot_time = time.time()
last_target_time = time.time()
last_save_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_scan',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
#print("trans" + repr(trans))
run_name = 'runJ223_5/'
#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
euler_rot = tf.transformations.euler_from_quaternion(rot)
#GP_Map.Calc_GP_Local(DwellTime_Map, CPS_Map, [trans[0], trans[1], np.degrees(euler_rot[2])])
#GP_Map.last_update = time.time()
#print("Max of GP Map %f: " %(np.max(GP_Map.grid)))
last_gp_time = time.time()
#print("Max Grid Value: " + repr(CPS_Map.grid.max()))
#Mask_Map.plot_map()
plot_rate = 1
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([trans[0], trans[1]], [0,5])
#print("GP var max: %f" %(np.max(GP_Map.grid2)))
#Binary_Cost_Map.plot_map([trans[0], trans[1]], [0,5])
#Q_Map.plot_map([trans[0], trans[1]], [-14,14])
#Q2_Map.plot_map([trans[0], trans[1]], [0,4])
#Cost_Map.plot_map([trans[0], trans[1]], [0,100])
#Cost_Map.plot_map([trans[0], trans[1]], [0,100])
#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:
euler_rot = tf.transformations.euler_from_quaternion(rot)
#[targetPosition, Q_orig, Q_binary] = TargetSelection([trans[0], trans[1], euler_rot[2]], GP_Map, Mask_Map, Qthreshold, [0,0])
[targetPosition, Q_orig,
Q_binary] = TargetSelection([trans[0], trans[1], euler_rot[2]],
GP_Map, Mask_Map, Qthreshold)
Q_Map.updateGrid(Q_orig)
Q2_Map.updateGrid(Q_binary)
last_target_time = time.time()
#targetPosition = [0,0,0]
#Publish target commands
euler_rot = tf.transformations.euler_from_quaternion(rot)
GoalQuaternion = tf.transformations.quaternion_from_euler(
0, 0, targetPosition[2])
#print(targetPosition)
GoalSimpletoPublish.pose.position.x = targetPosition[0]
GoalSimpletoPublish.pose.position.y = targetPosition[1]
GoalSimpletoPublish.pose.orientation.x = GoalQuaternion[0]
GoalSimpletoPublish.pose.orientation.y = GoalQuaternion[1]
GoalSimpletoPublish.pose.orientation.z = GoalQuaternion[2]
GoalSimpletoPublish.pose.orientation.w = GoalQuaternion[3]
GoaltoPublish.goal.target_pose.pose.position.x = targetPosition[0]
GoaltoPublish.goal.target_pose.pose.position.y = targetPosition[1]
GoaltoPublish.goal.target_pose.pose.orientation.x = GoalQuaternion[
0]
GoaltoPublish.goal.target_pose.pose.orientation.y = GoalQuaternion[
1]
GoaltoPublish.goal.target_pose.pose.orientation.z = GoalQuaternion[
2]
GoaltoPublish.goal.target_pose.pose.orientation.w = GoalQuaternion[
3]
if True: #Make true to save data
np.save(
'/home/bloodhound/catkin_ws/src/radiation_mapping/src/' +
'Map_Data/' + run_name + 'Location_' + repr(time.time()),
[trans[0], trans[1], euler_rot[2]])
#GoaltoPublish.pose.orientation = tf.transformations.quaternion_from_euler(targetPosition[0], targetPosition[1], targetPosition[2])
#GoalSimplePublisher.publish(GoalSimpletoPublish) #Disable for teleop control
#GoalPublisher.publish(GoaltoPublish) #Disable for teleop control
#Save data for later plotting
save_rate = 1
if time.time() > last_save_time + 1 / save_rate:
#Save all desired maps
#Save Data
if True: #Make true to save data
#np.save('Map_Data/' + run_name + 'GP_Map_' + repr(GP_Map.last_update), GP_Map.grid)
#np.save('Map_Data/' + run_name + 'GP_Map_var' + repr(GP_Map.last_update), GP_Map.grid2)
#np.save('Map_Data/' + run_name + 'Count_Map_' + repr(Count_Map.last_update), Count_Map.grid)
#np.save('Map_Data/' + run_name + 'DwellTime_Map_' + repr(DwellTime_Map.last_update), DwellTime_Map.grid)
#np.save('Map_Data/' + run_name + 'Q_Map_' + repr(Q_Map.last_update), Q_Map.grid)
np.save(
'/home/bloodhound/catkin_ws/src/radiation_mapping/src/' +
'Map_Data/' + run_name + 'Occ_Map_' +
repr(occupancy_Map.last_update), occupancy_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 Calc_GP(K, Ks, Kss, y_train, y_train_var, background, sigma_n=1):
#Performs GP calculation and returns vectors
y_train = y_train - background
print(np.shape(K + sigma_n**2 * np.diag(y_train_var)))
L = np.linalg.cholesky(K + sigma_n**2 * np.diag(y_train_var))
alpha = np.linalg.solve(
L.T, np.linalg.solve(L, y_train.reshape((len(y_train), 1))))
y_test = np.dot(Ks, alpha) + background
v = np.linalg.solve(L, Ks.T)
y_test_var = np.diag(Kss - np.dot(v.T, v))
return [np.reshape(y_test, (1, len(y_test)))[0], y_test_var]
MyMap = RadiationMap.Map()
MyMap_CPS = RadiationMap.Map()
Kss = Make_K_ss(MyMap)
fig, (ax1, ax2) = plt.subplots(1, 2)
#im1 = plt.imshow(K_ss, interpolation='none')
#plt.show(block = False)
testpnts = [[-2, -2, 5, 6], [-1, -2, 5, 5], [0, -2, 5, 10], [0.2, -2, 5, 0]]
plotpnt_x = []
plotpnt_val = []
for entry in testpnts:
MyMap.set_cell(entry[0], entry[1], entry[2])
MyMap_CPS.set_cell(entry[0], entry[1], entry[3])
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()
import matplotlib.image as mpimg
from matplotlib import cm
import matplotlib.mlab as mlab
#Define Classes
import RadiationMap
import RadiationSource
#Global counter variables
global DwellTime_Map #Dwell time at each map voxel in seconds
global Count_Map #Number of radiation coutns accumulated at each map voxel
global trans
global Binary_Cost_Map
global Mask_Map
DwellTime_Map = RadiationMap.Map()
Count_Map = RadiationMap.Map()
def ComputeGP(CPS_Map, lambda1=0.1):
#Use CPS map with parameters to compute GP estimates at every point
pass
def SimulationRadation(current_location, sources, delta_t):
rad = 0
for source in sources:
rad += source.GenerateRandomRadiation(current_location, delta_t)
def Calc_GP(K, Ks, Kss, y_train, y_train_var, background, sigma_n = 1):
#Performs GP calculation and returns vectors
y_train = y_train - background
L = np.linalg.cholesky(K + sigma_n**2 * np.diag(y_train_var))
alpha = np.linalg.solve(L.T, np.linalg.solve(L, y_train.reshape((len(y_train),1))))
y_test= np.dot(Ks, alpha) + background
v = np.linalg.solve(L, Ks.T)
y_test_var = np.diag(Kss - np.dot(v.T, v))
return [np.reshape(y_test,(1,len(y_test)))[0], y_test_var]
MyMap = RadiationMap.Map(height = 0.1)
MyMap_CPS = RadiationMap.Map(height = 0.1)
Kss = Make_K_ss(MyMap)
fig = plt.figure()
#im1 = plt.imshow(K_ss, interpolation='none')
#plt.show(block = False)
testpnts = [[-2,-2,5,6],[-1,-2,5,5],[0,-2,5,10]]
plotpnt_x = []
plotpnt_val = []
for entry in testpnts:
#Performs GP calculation and returns vectors
y_train = y_train - background
print(np.shape(K + sigma_n**2 * np.diag(y_train_var)))
L = np.linalg.cholesky(K + sigma_n**2 * np.diag(y_train_var))
alpha = np.linalg.solve(L.T, np.linalg.solve(L, y_train.reshape((len(y_train),1))))
y_test= np.dot(Ks, alpha) + background
v = np.linalg.solve(L, Ks.T)
y_test_var = np.diag(Kss - np.dot(v.T, v))
return [np.reshape(y_test,(1,len(y_test)))[0], y_test_var]
MyMap = RadiationMap.Map()
MyMap_CPS = RadiationMap.Map()
MyMap_GP = RadiationMap.GP_Map()
testpnts = [[-2,-2,5,6],[-1,-2,5,5],[0,0,2,10],[1.1,0,2,3]]
#testpnts = []
plotpnt_x = []
plotpnt_val = []
for entry in testpnts:
MyMap.set_cell(entry[0], entry[1], entry[2])
MyMap_CPS.set_cell(entry[0], entry[1], entry[3])
plotpnt_x.append(entry[0])
plotpnt_val.append(entry[3])
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()
# im_ani = animation.ArtistAnimation(fig, ims, interval = 500, repeat_delay = 3000, blit = True)
# im_ani.save('vidTest.mp4',metadata = {'artist':'Peter'})
t1 = time.time()
totaltime = []
cumtime = []
t2 = time.time()
for i in range(0, 20):
MapWidth = 10.001
GPMapParameters = [-MapWidth / 2, -MapWidth / 2, 0.05, MapWidth, MapWidth]
GPMap = RadiationMap.GP_Map(*GPMapParameters) #0.0089
Q = copy.deepcopy(GPMap.grid) #0.000058
currentPosition = [3, 3, 0]
TurnPenalty = 10.1
maxDist = 10
#Choose closest Q value by eulidean distance, with penalty for angle change
xyMap = GPMap.xyMap #grid map with each xy position of grid cell center
distMap = np.zeros([len(GPMap.grid), len(GPMap.grid[0])])
#Go over nearby cells first, try to find match
dt = 0.1
t = 0
#currentPosition = [np.random.rand()*3.8-1.9, np.random.rand()*3.8-1.9,np.random.rand()*360]
currentPosition = [0, 0, 0]
targetPosition = currentPosition
Sim_Rad = [0, 0, 0]
robotRadius = 0.1 #robot radius in meters
threshold = 1.6
frameCnt = 0
timer = [0] * 10
MapWidth = 10.0001
RecordMapParameters = [-MapWidth / 2, -MapWidth / 2, 0.05, MapWidth, MapWidth]
GPMapParameters = [-MapWidth / 2, -MapWidth / 2, 0.05, MapWidth, MapWidth]
#Maps
DwellTime_Map = RadiationMap.Map(*RecordMapParameters, dtype='float32')
Count_Map = RadiationMap.Map(*RecordMapParameters, dtype='int16')
Mask_Map = RadiationMap.Map(*RecordMapParameters, dtype='int8')
CPS_Map = RadiationMap.Map(*RecordMapParameters, dtype='float32')
GP_Map = RadiationMap.GP_Map(*GPMapParameters)
print("Maps Created")
print(DwellTime_Map.grid.shape)
Q_binary = GP_Map.grid
Q_orig = GP_Map.grid
Mask_Map.grid += 1
for i in range(0, len(Mask_Map.grid)):
Mask_Map.grid[i][0] = 0
Mask_Map.grid[i][len(Mask_Map.grid) - 1] = 0
Mask_Map.grid[0][i] = 0
Mask_Map.grid[len(Mask_Map.grid) - 1][i] = 0