def e3poseCb(self, msg):
i = 2
lat = msg.latitude
lon = msg.longitude
# convert gps to grid position in ENU
x_enu, y_enu = hp.LLA_local_deltaxy(self.lat0, self.lon0, lat, lon)
p = Point32(x_enu, y_enu, 0.0)
self.e_msg.enemy_position[i] = p
# get sector
self.e_msg.enemy_sectors[i] = self.enu2sector(x_enu, y_enu, 0.0)
def d3poseCb(self, msg):
i = 2
lat = msg.latitude
lon = msg.longitude
#print "d lat/long: ", lat, lon
# convert gps to grid position in ENU
x_enu, y_enu = hp.LLA_local_deltaxy(self.lat0, self.lon0, lat, lon)
#print "x/y: ", x_enu, y_enu
p = Point32(x_enu, y_enu, 0.0)
self.d_msg.defenders_position[i] = p
# get sector
self.d_msg.defenders_sectors[i] = self.enu2sector(x_enu, y_enu, 0.0)
def boundSp(self, dx, dy):
if self.use_gps:
dx_g, dy_g = self.uti.localENU2GlobalENU(dx, dy)
lat, lon = hp.local_deltaxy_LLA(self.my_lat, self.my_lon, dy_g,
dx_g) # x/y siwtch for NED
# get position in grid
x, y = self.uti.global2local_ENU(lat, lon)
#x,y = hp.LLA_local_deltaxy(self.lat0, self.lon0, lat, lon)
if x < self.fence_x_min:
x = self.fence_x_min + self.safety_margin * self.uti.grid_side_length
if x > self.fence_x_max:
x = self.fence_x_max
if y < self.fence_y_min:
y = self.fence_y_min + self.safety_margin * self.uti.grid_side_length
if y > self.fence_y_max:
y = self.fence_y_max
# convert to mavros local_poistion
lat, lon = self.uti.local2global_GPS(x, y)
#lat, lon = hp.local_deltaxy_LLA(self.lat0, self.lon0, y, x) # x/y siwtch for NED
x, y = hp.LLA_local_deltaxy(self.my_lat, self.my_lon, lat, lon)
# x,y here are the change in setpoint, not the setpoint directly
return (x, y)
else:
x = dx
y = dy
if x < self.fence_x_min:
x = self.fence_x_min
if x > self.fence_x_max:
x = self.fence_x_max
if y < self.fence_y_min:
y = self.fence_y_min
if y > self.fence_y_max:
y = self.fence_y_max
# x,y here are the setpoints directly
return (x, y)
def get_grid_pos(self):
self.grid_pos_x, self.grid_pos_y = hp.LLA_local_deltaxy(
self.lat0, self.lon0, self.my_lat, self.my_lon)