Exemple #1
0
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)
Exemple #3
0
                                          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]))
Exemple #4
0
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, [