def __init__(self):
self.node_name = 'PreRun'
rospy.init_node(self.node_name, anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
self.auv_handler = AuvHandler(self.node_name, "PreRun")
rospy.Subscriber("/Vehicle/Out/Temperature_filtered", Temperature,
self.TemperatureCB)
rospy.Subscriber("/Vehicle/Out/Salinity_filtered", Salinity,
self.SalinityCB)
rospy.Subscriber("/Vehicle/Out/EstimatedState_filtered",
EstimatedState, self.EstimatedStateCB)
self.speed = 1.5 # m/s
self.depth = 0.0 # meters
self.last_state = "unavailable"
self.rate.sleep()
# self.auv_handler.setWaypoint(deg2rad(lat4), deg2rad(lon4))
# self.auv_handler.setWaypoint(Path_PreRun[0][0], Path_PreRun[0][1], Path_PreRun[0][2])
self.auv_handler.setWaypoint(Path_PreRun[0][0], Path_PreRun[0][1])
self.init = True
self.currentTemperature = 0.0
self.currentSalinity = 0.0
self.vehicle_pos = [0, 0, 0]
def __init__(self):
self.node_name = 'ES3D1'
rospy.init_node(self.node_name, anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
self.auv_handler = AuvHandler(self.node_name, "ES3D1")
rospy.Subscriber("/Vehicle/Out/Temperature_filtered", Temperature,
self.TemperatureCB)
rospy.Subscriber("/Vehicle/Out/Salinity_filtered", Salinity,
self.SalinityCB)
rospy.Subscriber("/Vehicle/Out/EstimatedState_filtered",
EstimatedState, self.EstimatedStateCB)
self.speed = 1.6 # m/s
self.depth = 0.0 # meters
self.last_state = "unavailable"
self.rate.sleep()
# self.auv_handler.setWaypoint(deg2rad(lat4), deg2rad(lon4))
# self.auv_handler.setWaypoint(deg2rad(lat_start), deg2rad(lon_start))
self.init = True
self.currentTemperature = 0.0
self.currentSalinity = 0.0
self.vehicle_pos = [0, 0, 0]
self.surfacing = False
# Move to the starting location
print(xstart, ystart, zstart)
self.auv_handler.setWaypoint(deg2rad(lat_start), deg2rad(lon_start),
depth_obs[zstart])
def __init__(self):
self.node_name = 'gp_example'
rospy.init_node(self.node_name, anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
# config = "/home/tore/software/adaframe_ws/src/adaframe_examples/scripts/gp_example.ini"
self.auv_handler = AuvHandler(self.node_name, "empty")
# self.auv_handler = AuvHandler(self.node_name,config)
rospy.Subscriber("/Vehicle/Out/Temperature_filtered", Temperature,
self.TemperatureCB)
rospy.Subscriber("/Vehicle/Out/EstimatedState_filtered",
EstimatedState, self.EstimatedStateCB)
self.speed = 4.9 #m/s
self.depth = 0.0 #meters
self.init = False
self.vehicle_pos = [0, 0, 0]
self.last_state = "waiting"
# self.last_state = "unavailable"
#Earths circumference
self.circumference = 40075000
############ GP values ##############
self.origin = np.array([63.44045785, 10.35580542]) ## DEFINE ORIGIN
self.origin_rad = self.origin * np.pi / 180.0
#make a rectangle, in meters, [north, east]
self.offsets = np.array([200.0, 200.0])
self.gp_resolution = 11 ## number of grid points
self.meters_per_grid = self.offsets / self.gp_resolution
self.xgrid = np.linspace(0, self.offsets[0], self.gp_resolution)
self.ygrid = np.linspace(0, self.offsets[1], self.gp_resolution)
self.iteration = 0
self.measurements = []
#Set the first WP in the middle of the box
self.first_wp = self.offsets / 2.0
self.first_wp_rad = self.grid2wp(self.first_wp[0], self.first_wp[1])
self.auv_handler.setWaypoint(self.first_wp_rad[0],
self.first_wp_rad[1],
speed=self.speed,
z=self.depth)
self.rate.sleep(
) # it will also change the init state to True, after multiple trials found out
while not self.init:
self.rate.sleep()
rospy.loginfo("initiated")
class AUV:
def __init__(self):
self.node_name = 'AUV_ES2D'
rospy.init_node(self.node_name, anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
self.auv_handler = AuvHandler(self.node_name, "empty")
self.speed = 1.9 #m/s
self.depth = 0.0 #meters
self.last_state = "unavailable"
self.rate.sleep()
self.auv_handler.setWaypoint(origin_lat, origin_lon)
self.init = True
self.counter = 0
def test(self):
counter = 0
while not rospy.is_shutdown():
if self.init:
if self.auv_handler.getState() == "waiting":
print("Arrived the current location")
if counter < N_steps:
print("Move to new way point")
counter = counter + 1
self.auv_handler.setWaypoint(lat[counter],
lon[counter],
dep[counter])
self.last_state = self.auv_handler.getState()
self.auv_handler.spin()
self.rate.sleep()
def __init__(self):
self.node_name = 'AUV_ES2D'
rospy.init_node(self.node_name, anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
self.auv_handler = AuvHandler(self.node_name, "empty")
self.speed = 1.9 #m/s
self.depth = 0.0 #meters
self.last_state = "unavailable"
self.rate.sleep()
self.auv_handler.setWaypoint(origin_lat, origin_lon)
self.init = True
self.counter = 0
def __init__(self):
self.node_name = 'MASCOT'
rospy.init_node(self.node_name, anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
self.auv_handler = AuvHandler(self.node_name, "MASCOT")
rospy.Subscriber("/Vehicle/Out/Temperature_filtered", Temperature,
self.TemperatureCB)
rospy.Subscriber("/Vehicle/Out/Salinity_filtered", Salinity,
self.SalinityCB)
rospy.Subscriber("/Vehicle/Out/EstimatedState_filtered",
EstimatedState, self.EstimatedStateCB)
self.speed = 1.2 # m/s
self.depth = 0.0 # meters
self.last_state = "unavailable"
self.rate.sleep()
self.init = True
self.currentTemperature = 0.0
self.currentSalinity = 0.0
self.vehicle_pos = [0, 0, 0]
self.maxtime_underwater = 600
self.popup_duration = 90
self.sms_pub_ = rospy.Publisher("/IMC/In/Sms", Sms, queue_size=10)
self.phone_number = "+351969459285"
self.iridium_destination = "manta-ntnu-1"
def __init__(self):
self.node_name = 'LAUV_HARALD'
rospy.init_node(self.node_name,anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
self.auv_handler = AuvHandler(self.node_name,"empty")
self.speed = 1.9 #m/s
self.depth = 0.0 #meters
self.lat_root = 63.44187717 * np.pi / 180.0
self.lon_root = 10.3636388 * np.pi / 180.0
self.dgrid = 80
self.circumference = 40075000
self.leftcornerlat = self.lat_root + 2000 / self.circumference * 2 * np.pi
self.leftcornerlon = self.lon_root
self.path_row = np.loadtxt("path_row.txt")
self.path_col = np.loadtxt("path_col.txt")
self.path_lat = []
self.path_lon = []
# print(self.path_row)
# print(self.path_col)
for i in range(self.path_row.shape[0]):
# print(self.path_row[i], self.path_col[i])
self.path_dlat, self.path_dlon = self.ind2wp(self.path_row[i], self.path_col[i])
self.path_rlat, self.path_rlon = self.dist2rad(self.path_dlat, self.path_dlon, self.lat_root)
self.path_lat.append(self.lat_root + self.path_rlat)
self.path_lon.append(self.lon_root + self.path_rlon)
# print(self.path_lat)
# print(self.path_lon)
self.last_state = "unavailable"
self.rate.sleep()
self.init = True
self.counter = 0
self.counterMAX = self.path_row.shape[0]
class AUV:
def __init__(self):
self.node_name = 'AUV_ES2D'
rospy.init_node(self.node_name,anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
self.auv_handler = AuvHandler(self.node_name,"empty")
self.speed = 1.9 #m/s
self.depth = 0.0 #meters
self.last_state = "unavailable"
self.rate.sleep()
self.auv_handler.setWaypoint(origin_lat, origin_lon)
self.init = True
self.counter = 0
def test(self):
counter = 0
while not rospy.is_shutdown():
if self.init:
if self.auv_handler.getState() == "waiting":
print("Arrived the current location, move to next one")
self.last_state = self.auv_handler.getState()
self.auv_handler.spin()
print(counter)
counter = counter + 1
print('AHA')
self.rate.sleep()
def run(self):
print('hello world')
def __init__(self):
Grid.__init__(self)
self.node_name = 'MASCOT'
rospy.init_node(self.node_name, anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
self.auv_handler = AuvHandler(self.node_name, "MASCOT")
rospy.Subscriber("/Vehicle/Out/Temperature_filtered", Temperature,
self.TemperatureCB)
rospy.Subscriber("/Vehicle/Out/Salinity_filtered", Salinity,
self.SalinityCB)
rospy.Subscriber("/Vehicle/Out/EstimatedState_filtered",
EstimatedState, self.EstimatedStateCB)
self.speed = 1.6 # m/s
self.depth = 0.0 # meters
self.last_state = "unavailable"
self.rate.sleep()
self.init = True
self.currentTemperature = 0.0
self.currentSalinity = 0.0
self.vehicle_pos = [0, 0, 0]
self.surfacing = False
self.surfacing_time = 25 # surface time, [sec]
class PreRun:
def __init__(self):
self.node_name = 'PreRun'
rospy.init_node(self.node_name, anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
self.auv_handler = AuvHandler(self.node_name, "PreRun")
rospy.Subscriber("/Vehicle/Out/Temperature_filtered", Temperature,
self.TemperatureCB)
rospy.Subscriber("/Vehicle/Out/Salinity_filtered", Salinity,
self.SalinityCB)
rospy.Subscriber("/Vehicle/Out/EstimatedState_filtered",
EstimatedState, self.EstimatedStateCB)
self.speed = 1.5 #m/s
self.depth = 0.0 #meters
self.last_state = "unavailable"
self.rate.sleep()
self.auv_handler.setWaypoint(deg2rad(lat4), deg2rad(lon4))
self.init = True
self.currentTemperature = 0.0
self.currentSalinity = 0.0
self.vehicle_pos = [0, 0, 0]
def TemperatureCB(self, msg):
self.currentTemperature = msg.value.data
def SalinityCB(self, msg):
self.currentSalinity = msg.value.data
def EstimatedStateCB(self, msg):
offset_north = msg.lat.data - deg2rad(lat4)
offset_east = msg.lon.data - deg2rad(lon4)
circumference = 40075000.0
N = offset_north * circumference / (2.0 * np.pi)
E = offset_east * circumference * np.cos(deg2rad(lat4)) / (2.0 * np.pi)
D = msg.z.data
self.vehicle_pos = [N, E, D]
def run(self):
counter = 0
counter_datasave = 0
counter_total_datasaved = 0
timestamp = 0
while not rospy.is_shutdown():
if self.init:
data_timestamp.append(timestamp)
data_temperature.append(self.currentTemperature)
data_salinity.append(self.currentSalinity)
data_x.append(self.vehicle_pos[0])
data_y.append(self.vehicle_pos[1])
data_z.append(self.vehicle_pos[-1])
data_lat.append(lat4)
data_lon.append(lon4)
if counter_datasave >= 10:
save_data(datapath, data_timestamp, data_lat, data_lon,
data_x, data_y, data_z, data_salinity,
data_temperature)
print("Data saved {:d} times".format(
counter_total_datasaved))
counter_datasave = 0
counter_total_datasaved = counter_total_datasaved + 1
timestamp = timestamp + 1
counter_datasave = counter_datasave + 1
if self.auv_handler.getState() == "waiting":
print("Arrived the current location")
save_data(datapath, data_timestamp, data_lat, data_lon,
data_x, data_y, data_z, data_salinity,
data_temperature)
counter_total_datasaved = counter_total_datasaved + 1
print("Data saved {:02d} times".format(
counter_total_datasaved))
if counter < N_steps:
print(
"Move to new way point, lat: {:.2f}, lon: {:.2f}, depth: {:.2f}"
.format(Path_PreRun[counter][0],
Path_PreRun[counter][1],
Path_PreRun[counter][-1]))
self.auv_handler.setWaypoint(Path_PreRun[counter][0],
Path_PreRun[counter][1],
Path_PreRun[counter][-1])
if Path_PreRun[counter][-1] == 0:
for i in range(60):
print(i)
print("Sleep {:d} seconds".format(i))
self.auv_handler.spin(
) # publishes the reference, stay on the surface
self.rate.sleep() #
counter = counter + 1
else:
save_data(datapath, data_timestamp, data_lat, data_lon,
data_x, data_y, data_z, data_salinity,
data_temperature)
counter_total_datasaved = counter_total_datasaved + 1
print("Data saved {:d} times".format(
counter_total_datasaved))
rospy.signal_shutdown("Mission completed!!!")
self.last_state = self.auv_handler.getState()
self.auv_handler.spin()
self.rate.sleep()
class ES3D1:
def __init__(self):
self.node_name = 'ES3D1'
rospy.init_node(self.node_name, anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
self.auv_handler = AuvHandler(self.node_name, "ES3D1")
rospy.Subscriber("/Vehicle/Out/Temperature_filtered", Temperature, self.TemperatureCB)
rospy.Subscriber("/Vehicle/Out/Salinity_filtered", Salinity, self.SalinityCB)
rospy.Subscriber("/Vehicle/Out/EstimatedState_filtered", EstimatedState, self.EstimatedStateCB)
self.speed = 1.6 # m/s
self.depth = 0.0 # meters
self.last_state = "unavailable"
self.rate.sleep()
self.auv_handler.setWaypoint(deg2rad(lat4), deg2rad(lon4))
# self.auv_handler.setWaypoint(lat_start, lon_start, depth_obs[zstart])
self.init = True
self.currentTemperature = 0.0
self.currentSalinity = 0.0
self.vehicle_pos = [0, 0, 0]
def TemperatureCB(self, msg):
self.currentTemperature = msg.value.data
def SalinityCB(self, msg):
self.currentSalinity = msg.value.data
def EstimatedStateCB(self, msg):
offset_north = msg.lat.data - deg2rad(lat4)
offset_east = msg.lon.data - deg2rad(lon4)
circumference = 40075000.0
N = offset_north * circumference / (2.0 * np.pi)
E = offset_east * circumference * np.cos(deg2rad(lat4)) / (2.0 * np.pi)
D = msg.z.data
self.vehicle_pos = [N, E, D]
def run(self):
# ====== Setup section =====
counter = 0
counter_waypoint = 0
counter_datasave = 0
counter_total_datasaved = 0
timestamp = 0
# ====== Setup section =====
while not rospy.is_shutdown():
if self.init:
data_timestamp.append(timestamp)
data_temperature.append(self.currentTemperature)
data_salinity.append(self.currentSalinity)
data_x.append(self.vehicle_pos[0])
data_y.append(self.vehicle_pos[1])
data_z.append(self.vehicle_pos[-1])
data_lat.append(lat4)
data_lon.append(lon4)
if counter_datasave >= 10:
save_data(datapath, data_timestamp, data_lat, data_lon, data_x, data_y, data_z, data_salinity, data_temperature)
print("Data saved {:d} times".format(counter_total_datasaved))
counter_datasave = 0
counter_total_datasaved = counter_total_datasaved + 1
timestamp = timestamp + 1
counter_datasave = counter_datasave + 1
if self.auv_handler.getState() == "waiting":
if counter_waypoint == 0:
self.auv_handler.setWaypoint(deg2rad(lat_start), deg2rad(lon_start), depth_start)
else:
xcand, ycand, zcand = find_candidates_loc(xnow, ynow, znow, N1, N2, N3)
t1 = time.time()
xnext, ynext, znext = find_next_EIBV_1D(xcand, ycand, zcand,
xnow, ynow, znow,
xpre, ypre, zpre,
N1, N2, N3, Sigma_cond,
mu_cond, tau_sal, Threshold_S)
t2 = time.time()
t_elapsed.append(t2 - t1)
print("It takes {:.2f} seconds to compute the next waypoint".format(t2 - t1))
print("next is ", xnext, ynext, znext)
lat_next, lon_next = xy2latlon(xnext, ynext, origin, distance, alpha)
depth_next = depth_obs[znext]
ind_next = ravel_index([xnext, ynext, znext], N1, N2, N3)
F = np.zeros([1, N])
F[0, ind_next] = True
print("Arrived the starting location")
save_data(datapath, data_timestamp, data_lat, data_lon, data_x, data_y, data_z, data_salinity, data_temperature)
counter_total_datasaved = counter_total_datasaved + 1
print("Data saved {:02d} times".format(counter_total_datasaved))
print("Move to new way point, lat: {:.2f}, lon: {:.2f}, depth: {:.2f}".format(lat_next, lon_next, depth_next))
sal_sampled = np.mean(data_salinity[-10:]) # take the past ten samples and average
mu_cond, Sigma_cond = GPupd(mu_cond, Sigma_cond, R, F, sal_sampled)
xpre, ypre, zpre = xnow, ynow, znow
xnow, ynow, znow = xnext, ynext, znext
path.append([xnow, ynow, znow])
path_cand.append([xcand, ycand, zcand])
coords.append([lat_next, lon_next])
mu.append(mu_cond)
Sigma.append(Sigma_cond)
save_ESdata(datapath, path, path_cand, coords, mu, Sigma, t_elapsed)
# Move to the next waypoint
self.auv_handler.setWaypoint(deg2rad(lat_next), deg2rad(lon_next), depth_next)
counter_waypoint = counter_waypoint + 1
print("Waypoint is ", counter_waypoint)
if counter_waypoint >= N_steps:
rospy.signal_shutdown("Mission completed!!!")
class GPExample:
def __init__(self):
self.node_name = 'gp_example'
rospy.init_node(self.node_name,anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
# config = "/home/tore/software/adaframe_ws/src/adaframe_examples/scripts/gp_example.ini"
self.auv_handler = AuvHandler(self.node_name)
# self.auv_handler = AuvHandler(self.node_name,config)
rospy.Subscriber("/Vehicle/Out/Temperature_filtered", Temperature, self.TemperatureCB)
rospy.Subscriber("/Vehicle/Out/EstimatedState_filtered", EstimatedState, self.EstimatedStateCB)
self.speed = 1.9 #m/s
self.depth = 0.0 #meters
self.init = False
self.vehicle_pos = [0,0,0]
self.last_state = "unavailable"
#Earths circumference
self.circumference = 40075000
############ GP values ##############
self.origin = np.array([63.44045785,10.35580542]) ## DEFINE ORIGIN
self.origin_rad = self.origin*np.pi/180.0
#make a rectangle, in meters, [north, east]
self.offsets = np.array([200.0, 200.0])
self.gp_resolution = 11 ## GRID
self.meters_per_grid = self.offsets/self.gp_resolution
self.xgrid = np.linspace(0,self.offsets[0],self.gp_resolution)
self.ygrid = np.linspace(0,self.offsets[1],self.gp_resolution)
self.iteration = 0
self.measurements = []
#Set the first WP in the middle of the box
self.first_wp = self.offsets/2.0
self.first_wp_rad = self.grid2wp(self.first_wp[0],self.first_wp[1])
self.auv_handler.setWaypoint(self.first_wp_rad[0],self.first_wp_rad[1], speed = self.speed, z = self.depth)
self.rate.sleep()
while not self.init:
self.rate.sleep()
rospy.loginfo("initiated")
def TemperatureCB(self, msg):
if self.init:
self.measurements.append([self.vehicle_pos,msg.value.data]) # SImulate CTD sensor
def EstimatedStateCB(self,msg):
offset_north = msg.lat.data - self.origin_rad[0]
offset_east = msg.lon.data - self.origin_rad[1]
circumference = 40075000.0
N = offset_north * circumference / (2.0 * np.pi)
E = offset_east * circumference * np.cos(self.origin_rad[0]) / (2.0 * np.pi)
D = msg.z.data
self.vehicle_pos = [N, E, D]
if not self.init:
self.init = True
def grid2wp(self,x,y):
lat = self.origin_rad[0] + x*np.pi*2.0/self.circumference
lon = self.origin_rad[1] + y*np.pi*2.0/(self.circumference*np.cos(self.origin_rad[0]))
return lat,lon
def updateWP(self):
values = np.zeros((self.gp_resolution**2,2))
for measurement in self.measurements:
x = int(np.floor(measurement[0][0]/self.meters_per_grid[0]))
y = int(np.floor(measurement[0][1]/self.meters_per_grid[1]))
values[max(0,min(x + y*self.gp_resolution,self.gp_resolution**2-1)),0] += measurement[1]
values[max(0,min(x + y*self.gp_resolution,self.gp_resolution**2-1)),1] += 1
x = []
y = []
temperature = []
i = 0
#Clear previous measurements
self.measurements = []
for value in values:
if value[1] >= 1:
temperature.append(value[0]/value[1])
x.append((i%self.gp_resolution)*self.meters_per_grid[0])
y.append(np.round(i/self.gp_resolution)*self.meters_per_grid[1])
vpos = [(i%self.gp_resolution)*self.meters_per_grid[0],np.round(i/self.gp_resolution)*self.meters_per_grid[1],0.0]
temperatureval = value[0]/value[1]
self.measurements.append([vpos,temperatureval])
i += 1
# Create ordinary kriging object:
OK = OrdinaryKriging(x,y,temperature,variogram_model="linear",verbose=True,enable_plotting=False)
z1, ss1 = OK.execute("grid", self.xgrid, self.ygrid)
fig1, ax1 = plt.subplots(1,2,figsize = (10,4))
cs = ax1[0].contourf(self.ygrid ,self.xgrid, z1, cmap = "coolwarm")
ax1[0].set_title('Interpolated Temperature [°C]')
ax1[0].set_xlabel("Easting [m]")
ax1[0].set_ylabel("Northing [m]")
#ax1.clabel(cs, fontsize=9, inline=1)
fig1.colorbar(cs,ax=ax1[0])
#ax1[0].plot(y, x, "+w")
art = ax1[1].contourf(self.ygrid,self.xgrid, ss1, cmap = "YlGn_r")
ax1[1].set_title("Estimated Variance")
ax1[1].set_xlabel("Easting [m]")
ax1[1].set_ylabel("Northing [m]")
plt.colorbar(art,ax=ax1[1])
#change this to wherever you want your figures stored.
# pngstr = "/home/tore/software/adaframe_ws/figures/" + "gp_example_" + str(self.iteration) + ".png"
pngstr = "/home/yaoling/MASCOT/Integrate/adaframe/catkin_ws/src/adaframe_examples/fig/" + "gp_example_" + str(self.iteration) + ".png"
plt.savefig(pngstr)
#Choose the new waypoint
i = 0
maxvar = -100.0
for val in ss1.flatten():
if val > maxvar:
maxvar = val
x = (i%self.gp_resolution)*self.meters_per_grid[0]
y = np.floor(i/self.gp_resolution)*self.meters_per_grid[1]
i += 1
lat,lon = self.grid2wp(x,y)
self.auv_handler.setWaypoint(lat,lon)
self.iteration += 1
print("updatedWP")
def run(self):
while not rospy.is_shutdown():
if self.init:
if self.auv_handler.getState() == "waiting":
self.updateWP()
self.last_state = self.auv_handler.getState()
self.auv_handler.spin()
self.rate.sleep()
class GPExample:
def __init__(self):
self.node_name = 'gp_example'
rospy.init_node(self.node_name, anonymous=True)
self.rate = rospy.Rate(1) # 1Hz
# config = "/home/tore/software/adaframe_ws/src/adaframe_examples/scripts/gp_example.ini"
self.auv_handler = AuvHandler(self.node_name, "empty")
# self.auv_handler = AuvHandler(self.node_name,config)
rospy.Subscriber("/Vehicle/Out/Temperature_filtered", Temperature,
self.TemperatureCB)
rospy.Subscriber("/Vehicle/Out/EstimatedState_filtered",
EstimatedState, self.EstimatedStateCB)
self.speed = 4.9 #m/s
self.depth = 0.0 #meters
self.init = False
self.vehicle_pos = [0, 0, 0]
self.last_state = "waiting"
# self.last_state = "unavailable"
#Earths circumference
self.circumference = 40075000
############ GP values ##############
self.origin = np.array([63.44045785, 10.35580542]) ## DEFINE ORIGIN
self.origin_rad = self.origin * np.pi / 180.0
#make a rectangle, in meters, [north, east]
self.offsets = np.array([200.0, 200.0])
self.gp_resolution = 11 ## number of grid points
self.meters_per_grid = self.offsets / self.gp_resolution
self.xgrid = np.linspace(0, self.offsets[0], self.gp_resolution)
self.ygrid = np.linspace(0, self.offsets[1], self.gp_resolution)
self.iteration = 0
self.measurements = []
#Set the first WP in the middle of the box
self.first_wp = self.offsets / 2.0
self.first_wp_rad = self.grid2wp(self.first_wp[0], self.first_wp[1])
self.auv_handler.setWaypoint(self.first_wp_rad[0],
self.first_wp_rad[1],
speed=self.speed,
z=self.depth)
self.rate.sleep(
) # it will also change the init state to True, after multiple trials found out
while not self.init:
self.rate.sleep()
rospy.loginfo("initiated")
def TemperatureCB(self, msg): # what is CB here?
if self.init:
self.measurements.append([self.vehicle_pos,
msg.value.data]) # SImulate CTD sensor
def EstimatedStateCB(self, msg):
offset_north = msg.lat.data - self.origin_rad[0]
offset_east = msg.lon.data - self.origin_rad[1]
circumference = 40075000.0
N = offset_north * circumference / (2.0 * np.pi)
E = offset_east * circumference * np.cos(
self.origin_rad[0]) / (2.0 * np.pi)
D = msg.z.data
self.vehicle_pos = [N, E, D]
if not self.init:
self.init = True
def grid2wp(self, x, y):
lat = self.origin_rad[0] + x * np.pi * 2.0 / self.circumference
lon = self.origin_rad[1] + y * np.pi * 2.0 / (
self.circumference * np.cos(self.origin_rad[0]))
return lat, lon
def updateWP(self):
# values = np.zeros((self.gp_resolution**2,2))
for i in range(5):
x = i * 10
y = i * 10
print(x, y)
lat, lon = self.grid2wp(x, y)
print(lat, lon)
print(self.origin_rad[0])
print(self.origin_rad[1])
self.auv_handler.setWaypoint(lat, lon)
# for measurement in self.measurements:
# # print(len(measurement)) # measurement is a [[N, E, D], value] which is a length 2 list, but the first element is a length 3 list
# # print(len(measurement[0])) # length 3 list for containing position vector
# x = int(np.floor(measurement[0][0]/self.meters_per_grid[0])) # return it to the oridinal grid, i.e., which grid point in the grid
# y = int(np.floor(measurement[0][1]/self.meters_per_grid[1])) # return also in the east direction, y along east
# # print(x + y*self.gp_resolution)
# # print(x, y)
# # print(min(x + y*self.gp_resolution,self.gp_resolution**2-1))
# values[max(0,min(x + y*self.gp_resolution,self.gp_resolution**2-1)),0] += measurement[1]
# values[max(0,min(x + y*self.gp_resolution,self.gp_resolution**2-1)),1] += 1
# # print(values)
# # print(values)
# # print(values.shape)
# x = []
# y = []
# temperature = []
# i = 0
# #Clear previous measurements
# self.measurements = []
# for value in values:
# if value[1] >= 1:
# # print(value)
# temperature.append(value[0]/value[1])
# x.append((i%self.gp_resolution)*self.meters_per_grid[0]) # remember % is the module operator
# y.append(np.round(i/self.gp_resolution)*self.meters_per_grid[1])
# # print(x)
# # print(y)
# vpos = [(i%self.gp_resolution)*self.meters_per_grid[0],np.round(i/self.gp_resolution)*self.meters_per_grid[1],0.0]
# temperatureval = value[0]/value[1]
# self.measurements.append([vpos,temperatureval])
# i += 1
# print(temperature)
# # Create ordinary kriging object:
# OK = OrdinaryKriging(x,y,temperature,variogram_model="linear",verbose=True,enable_plotting=False)
# z1, ss1 = OK.execute("grid", self.xgrid, self.ygrid)
# fig1, ax1 = plt.subplots(1,2,figsize = (10,4))
# cs = ax1[0].contourf(self.ygrid ,self.xgrid, z1, cmap = "coolwarm")
# ax1[0].set_title('Interpolated Temperature [°C]')
# ax1[0].set_xlabel("Easting [m]")
# ax1[0].set_ylabel("Northing [m]")
# #ax1.clabel(cs, fontsize=9, inline=1)
# fig1.colorbar(cs,ax=ax1[0])
# #ax1[0].plot(y, x, "+w")
# art = ax1[1].contourf(self.ygrid,self.xgrid, ss1, cmap = "YlGn_r")
# ax1[1].set_title("Estimated Variance")
# ax1[1].set_xlabel("Easting [m]")
# ax1[1].set_ylabel("Northing [m]")
# plt.colorbar(art,ax=ax1[1])
# #change this to wherever you want your figures stored.
# # pngstr = "/home/tore/software/adaframe_ws/figures/" + "gp_example_" + str(self.iteration) + ".png"
# pngstr = "/home/yaoling/MASCOT/Integrate/adaframe/catkin_ws/src/adaframe_examples/fig/" + "gp_example_" + str(self.iteration) + ".png"
# plt.savefig(pngstr)
# #Choose the new waypoint
# i = 0
# maxvar = -100.0
# for val in ss1.flatten():
# if val > maxvar:
# maxvar = val
# x = (i%self.gp_resolution)*self.meters_per_grid[0]
# y = np.floor(i/self.gp_resolution)*self.meters_per_grid[1]
# i += 1
# lat,lon = self.grid2wp(x,y)
# self.auv_handler.setWaypoint(lat,lon)
# self.iteration += 1
print("updatedWP")
def run(self):
counter = 0
while not rospy.is_shutdown():
counter = counter + 1
print(counter)
# print(self.auv_handler.getState())
if self.init:
# if self.auv_handler.getState() == "waiting":
if True:
# print("here")
self.updateWP()
self.last_state = self.auv_handler.getState()
self.auv_handler.spin()
self.rate.sleep()