def distance(self, p1, p2):
uc = utmconv()
(hemisphere, zone, letter, easting1,
northing1) = uc.geodetic_to_utm(p1[0], p1[1])
(hemisphere, zone, letter, easting2,
northing2) = uc.geodetic_to_utm(p2[0], p2[1])
return sqrt((easting1 - easting2)**2 + (northing1 - northing2)**2)
def __init__(self, lat=0, lon=0, northing=0, easting=0, GPS_data=0):
self.lat = float(lat)
self.lon = float(lon)
self.northing = float(northing)
self.easting = float(easting)
self.altitude = 30
self.GPS_data = GPS_data
if self.GPS_data != 0:
self.lat = GPS_data.latitude
self.lon = GPS_data.longitude
self.altitude = GPS_data.altitude
# Default values:
self.hemisphere = 'N'
self.zone = 32
self.letter = 'U'
self.converter = utmconv()
if self.lat == 0 and self.lon == 0:
self.update_geo_coordinates()
if self.northing == 0 and self.easting == 0:
self.update_UTM_coordinates()
if self.GPS_data == 0:
self.GPS_data = GPS()
self.GPS_data.latitude = self.lat
self.GPS_data.longitude = self.lon
self.GPS_data.altitude = self.altitude
def __init__(self, filename, max_lines, relative_coordinates): self.i = 0 print 'Importing GPS data' file = open(filename, 'rb') file_content = csv.reader(file, delimiter=',') self.data = [] i = 0 uc = utmconv() origo_e = 0 origo_n = 0 for time, lat, lon, fix, sat, hdop in file_content: if fix > 0: # convert to UTM (hemisphere, zone, letter, easting, northing) = uc.geodetic_to_utm (float(lat),float(lon)) # use relative coordinates if relative_coordinates: if origo_e == 0: origo_e = easting origo_n = northing easting = 0 northing = 0 else: easting -= origo_e northing -= origo_n else: easting = 0 northing = 0 self.data.append([float(time), float(lat), float(lon), \ int(fix), int(sat), float(hdop), easting, northing]) i += 1 if max_lines > 0 and i == max_lines: break file.close() self.length = len(self.data) print '\tTotal samples: %d' % (self.length)
def __init__(self):
self.rate = rospy.Rate(update_interval)
self.bus = SMBus(1)
self.utmconv = utmconv()
self.recording = False
self.main_mode = ""
self.sub_mode = ""
self.lat = 0
self.lon = 0
self.alt = 0
self.mission_idx = 0
self.mission_len = 0
self.new_imu_reading = False
self.new_vel_reading = False
dt = 1/update_interval
self.state = np.zeros((4,1), float)
self.kalman = kalman.KalmanFilter(self.state, dt)
# self.state = np.zeros((6,1), float)
# self.kalman = kalman3.KalmanFilter(self.state, dt)
# self.state = np.zeros((9,1), float)
# self.kalman = kalman_acc3.KalmanFilter(self.state, dt)
self.local_position_ned = np.array([0,0,0])
self.vx = 0
self.vy = 0
self.vz = 0
self.ax = 0
self.ay = 0
self.az = 0
self.filtered_pos = np.empty((0,2), float)
self.local_position_landing = Point()
self.landing_target = np.array([0,0,0])
self.landing_coords = Point(1.17, 0.75, 0)
self.data = np.empty((0,4), float)
self.local_data = np.empty((0,3), float)
# self.filtered_data = np.empty((0,3), float)
self.filtered_data = np.empty((0,2), float)
self.rotation_matrix = np.array([[1,0],[0,1]])
rospack = rospkg.RosPack()
package_path = rospack.get_path("precision_landing")
self.data_path = package_path + "/data/"
rospy.Subscriber("/mavlink_pos", mavlink_lora_pos, self.on_drone_pos)
rospy.Subscriber("/telemetry/mission_info", telemetry_mission_info, self.on_mission_info)
rospy.Subscriber("/telemetry/heartbeat_status", telemetry_heartbeat_status, self.on_heartbeat_status)
rospy.Subscriber("/telemetry/imu_data_ned", telemetry_imu_ned, self.on_imu_data)
rospy.Subscriber("/telemetry/local_position_ned", telemetry_local_position_ned, self.on_local_pos)
self.sensor_data_pub = rospy.Publisher("/landing/sensor_data", precland_sensor_data, queue_size=0)
self.landing_target_pub = rospy.Publisher("/telemetry/set_landing_target", telemetry_landing_target, queue_size=0)
def __init__(self, serial_device, serial_baudrate, debug=False):
rospy.init_node(self.ROS_NODE_NAME, log_level=rospy.DEBUG)
rospy.loginfo('Started: ' + rospy.get_caller_id())
#serial_device = str(serial_device)
#serial_baudrate = int(float(serial_baudrate))
serial_device = rospy.get_param('/uav_restriction_node/serial_device')
serial_baudrate = rospy.get_param(
'/uav_restriction_node/serial_baudrate')
rospy.logdebug('Device %s, data type %s' %
(serial_device, type(serial_device)))
rospy.logdebug('Baud rate %s, data type %s' %
(serial_baudrate, type(serial_baudrate)))
self.debug = debug
rospy.loginfo('Configuring UTM for zone 32')
self.uc = utmconv()
self.uc.set_zone_override(32)
#Center positions
self.zeroEasting = 590734.045671
self.zeroNorthing = 6136563.68892
rospy.loginfo('Configuring arduino serial port')
self.ser_arduino = serial.Serial()
self.ser_arduino.baudrate = serial_baudrate #ARDUINO_BAUD_RATE
self.ser_arduino.port = serial_device #ARDUINO_PORT
#rospy.loginfo(ser)
rospy.loginfo('Opening arduino serial port')
try:
self.ser_arduino.open()
except serial.SerialException as e:
rospy.logerr('Could not open arduino serial port')
else:
with self.mutex_heading:
self.uav_heading_rad = 0
self.uav_heading_deg = self.uav_heading_rad * RAD2DEG
if rospy.get_param('/uav_restriction_node/emulate_arduino_send'):
r = rospy.Rate(0.5) # 10hz
while not rospy.is_shutdown():
rospy.loginfo('Sending')
self.send_arduino_message(0, 20, 30, 40, 50, 60)
r.sleep()
rospy.Subscriber(ROS_POSE_TOPIC, mavlink_lora_pos,
self.callback_pos)
rospy.Subscriber(ROS_ATTITUDE_TOPIC, mavlink_lora_attitude,
self.callback_attitude)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
rospy.loginfo('Closing arduino serial port')
while self.ser_arduino.is_open:
self.ser_arduino.close()
rospy.loginfo('Arduino serial port closed')
rospy.loginfo('Stopped ' + rospy.get_caller_id())
def geodetic_to_utm(geo_coordinates):
utm_values = np.array
uc = utmconv()
for point in geo_coordinates:
new_utm_values = uc.geodetic_to_utm(point[0], point[1])
# The first time, the exception is raised and the array is initialized.
try:
utm_values = np.vstack((utm_values, new_utm_values))
except ValueError:
utm_values = new_utm_values
return utm_values
def utm_to_geodetic(utm_data):
uc = utmconv()
geodetic_coordinates = np.array
for point in utm_data:
new_geodetic_coordinates = uc.utm_to_geodetic(str(utm_data[:,0][0]),
int(utm_data[:,1][0]),
float(utm_data[:,3][0]),
float(utm_data[:,4][0]))
# The first time, the exception is raised and the array is initialized.
try:
geodetic_coordinates = np.vstack((geodetic_coordinates,
new_geodetic_coordinates))
except ValueError:
geodetic_coordinates = new_geodetic_coordinates
return geodetic_coordinates
def __init__(self):
rospy.loginfo(rospy.get_name() + ": Start")
#Get parameters
#self.got_dev = rospy.get_param("~got_device", "/dev/drone_cage_got_in")
#self.aq_dev = rospy.get_param("~aq_device", "/dev/drone_cage_aq_out")
#Serial communications
serial_error = False
try:
self.serialIn = serial.Serial(
port="/dev/ttyUSB0",
baudrate=57600) #(port = self.got_dev, baudrate = 57600)
except Exception as e:
rospy.logerr(
rospy.get_name() +
": Unable to open serial device") # %s" % self.got_dev)
serial_error = True
if serial_error == False:
try:
self.serialOut = serial.Serial(
port="/dev/ttyUSB1",
baudrate=57600) #(port = self.aq_dev, baudrate = 57600)
except Exception as e:
rospy.logerr(
rospy.get_name() +
": Unable to open serial device") # %s" % self.aq_dev)
serial_error = True
if serial_error == False:
#Position conversion class
self.uc = utmconv()
#Crc calculation class
self.crc = crc16()
#Center positions
self.zeroEasting = 590734.045671
self.zeroNorthing = 6136563.68892
#Update frequency
self.rate = 200 #5Hz
self.r = rospy.Rate(self.rate)
self.updater()
def main():
# Fixing random state for reproducibility
np.random.seed(19680801)
fig, ax = plt.subplots()
patches = []
utmcon = utmconv()
for int_id, val in dict_json.items():
coords = val["coordinates"]
if val["geometry"] == "polygon":
coords = coords.split(" ")
i = 0
for a in coords:
coords[i] = a.split(',')
i += 1
new_c = []
for i in coords:
(hemisphere, zone, zlet, easting,
northing) = utmcon.geodetic_to_utm(float(i[0]), float(i[1]))
new_c.append([easting, northing])
polygon = Polygon(new_c, True)
patches.append(polygon)
print(easting, northing)
else:
coords = coords.split(',')
(hemisphere, zone, zlet, easting,
northing) = utmcon.geodetic_to_utm(float(coords[0]),
float(coords[1]))
circle = Circle((easting, northing), float(coords[2]))
#patches.append(circle)
colors = 100 * np.random.rand(len(patches))
p = PatchCollection(patches, alpha=0.4)
p.set_array(np.array(colors))
ax.add_collection(p)
plt.ylim(1141400, 1142400)
plt.xlim(332400, 333400)
fig.colorbar(p, ax=ax)
plt.show()
def utm_to_geodetic(utm_data):
uc = utmconv()
geodetic_coordinates = np.array
for point in utm_data:
# import pdb; pdb.set_trace()
new_geodetic_coordinates = uc.utm_to_geodetic(str(point[0]),
int(point[1]),
float(point[3]),
float(point[4]))
# The first time, the exception is raised and the array is initialized.
try:
geodetic_coordinates = np.vstack((geodetic_coordinates,
new_geodetic_coordinates))
except ValueError:
geodetic_coordinates = new_geodetic_coordinates
return geodetic_coordinates
def get_remove_outliers(self, points):
lat_data = points[:, 0]
lon_data = points[:, 1]
utm_easting = []
utm_northing = []
geo_lat = []
geo_lon = []
variable_distance = 0
# initialize variables
uc = utmconv()
old_e = 590686.5315567022
old_n = 6136529.126779066
# looping through file
for i in range(len(lat_data)):
lat = float(lat_data[i])
lon = float(lon_data[i])
(hemisphere, zone, letter, easting,
northing) = uc.geodetic_to_utm(lat, lon)
tmp_distance = sqrt((old_e - easting)**2 + (old_n - northing)**2)
if tmp_distance < 1 + variable_distance: # and time < 527.542:
utm_easting.append(easting)
utm_northing.append(northing)
geo_lat.append(lat)
geo_lon.append(lon)
variable_distance = 0
old_e = easting
old_n = northing
else:
variable_distance += 0.2
# self.plot_data(utm_easting, utm_northing, 'Data', 'Easting', 'Northing', 'Position')
points = np.column_stack((geo_lat, geo_lon))
return points
# 1000m
m = 1000
# geodetic reference coordinate
lat1 = 55.47
lon1 = 010.33
# Our geodetic reference coordinate
lat1_1 = 68.530855
lon1_1 = -089.827172
print ('First position [deg]:')
print (' latitude: %.8f' % (lat1))
print (' longitude: %.8f' % (lon1))
# instantiate utmconv class
uc = utmconv()
uc_1 = utmconv()
# convert from geodetic to UTM
(hemisphere, zone, letter, e1, n1) = uc.geodetic_to_utm (lat1,lon1)
print ('\nConverted from geodetic to UTM [m]')
print (' %d %c %.5fe %.5fn' % (zone, letter, e1, n1))
(hemisphere_1, zone_1, letter_1, e1_1, n1_1) = uc_1.geodetic_to_utm (lat1_1,lon1_1)
# now generating the second UTM coordinate
e2 = e1
n2 = n1 + m
e2_1 = e1_1 + m
n2_1 = n1_1
# convert back from UTM to geodetic
from utm import utmconv
from math import pi, cos, acos, sin
# define test position
#test_lat = 55.47
#test_lon = 010.33
test_lat = 42.160771
test_lon = 024.747739
print('Test position [deg]:')
print(' latitude: %.10f' % (test_lat))
print(' longitude: %.10f' % (test_lon))
#N55.47 E010.33
# instantiate utmconv class
#uc = utm.from_latlon(lat, lon)
uc = utmconv()
# convert from geodetic to UTM
(hemisphere, zone, letter, easting,
northing) = uc.geodetic_to_utm(test_lat, test_lon)
print('\nConverted from geodetic to UTM [m]')
print(' %d %c %.5fe %.5fn' % (zone, letter, easting, northing))
# convert back from UTM to geodetic
(lat, lon) = uc.utm_to_geodetic(hemisphere, zone, easting, northing)
print('\nConverted back from UTM to geodetic [deg]:')
print(' Original latitude: %.10f' % (lat))
print(' Original longitude: %.10f' % (lon))
lat11 = lat
lon11 = lon
lat = lat * (pi / (180))
def __init__(self):
rospy.sleep(2)
self.scenario = rospy.get_param("~scenario")
self.payload = {''}
self.kml = kml_no_fly_zones_parser(0)
self.static_filename = ''
self.geoditic_coords = []
self.utm_coords = []
self.coord_conv = utmconv()
self.utm_trafic_debug = 0
self.debug = 0
self.path_debug = 0
self.push_debug = 0
self.utm_coords = []
self.empty_map = []
self.map_ll_reference = []
self.map_res = 1
self.ll_dummy = ['N', 32, 'U', 466703, 6162091]
self.ur_dummy = ['N', 32, 'U', 469703, 6165091]
self.map_length = 0
self.map_width = 0
self.map_height = 0
self.path = []
self.post_payload = {
'uav_id': 3012,
'uav_auth_key':
'96ba4387cb37a2cbc5f05de53d5eab0c9583f1e102f8fe10ccab04c361234d6cd8cc47c0db4a46e569f03b61374745ebb433c84fac5f4bdfb8d89d2eb1d1ec0f',
'uav_op_status': 22,
'pos_cur_lat_dd': 0,
'pos_cur_lng_dd': 0,
'pos_cur_alt_m': 0,
'pos_cur_hdg_deg': 0,
'pos_cur_vel_mps': 0,
'pos_cur_gps_timestamp': 0,
'wp_next_lat_dd': 0,
'wp_next_lng_dd': 0,
'wp_next_alt_m': 0,
'wp_next_hdg_deg': 0,
'wp_next_vel_mps': 0,
'wp_next_eta_epoch': 0,
'uav_bat_soc': 0
}
self.standard_post_payload = {
'uav_id': 3012,
'uav_auth_key':
'96ba4387cb37a2cbc5f05de53d5eab0c9583f1e102f8fe10ccab04c361234d6cd8cc47c0db4a46e569f03b61374745ebb433c84fac5f4bdfb8d89d2eb1d1ec0f',
'uav_op_status': 22,
'pos_cur_lat_dd': 1,
'pos_cur_lng_dd': 1,
'pos_cur_alt_m': 1,
'pos_cur_hdg_deg': 1,
'pos_cur_vel_mps': 1,
'pos_cur_gps_timestamp': 1,
'wp_next_lat_dd': -1,
'wp_next_lng_dd': -1,
'wp_next_alt_m': -1,
'wp_next_hdg_deg': -1,
'wp_next_vel_mps': -1,
'wp_next_eta_epoch': -1,
'uav_bat_soc': 100
}
self.at_last_wp = 0
self.last_info_pub = time.time()
self.path_flag = False
self.latest_dynamic_data = self.get_dynamic_nfz()
self.published_first_dnfz = 0
#ROS STUFF
self.get_snfz_service = rospy.Service("/utm_parser/get_snfz",
get_snfz,
self.get_snfz_handler,
buff_size=10)
self.get_dnfz_service = rospy.Service("/utm_parser/get_dnfz",
get_dnfz,
self.get_dnfz_handler,
buff_size=10)
self.is_coord_free_service = rospy.Service("/utm_parser/is_coord_free",
is_coord_free,
self.check_coord_service,
buff_size=10)
self.get_rally_points = rospy.Service("/utm_parser/get_rally_points",
get_rally_points,
self.get_rally_points_handler,
buff_size=10)
#self.post_drone_service = rospy.Service("/utm_parser/post_drone_info", post_drone_info, self.post_drone_info_handler, buff_size=10) ##SUBSCRIBE
self.drone_info_sub = rospy.Subscriber("/drone_handler/DroneInfo",
DroneInfo,
self.post_drone_info_handler)
self.path_sub = rospy.Subscriber(
"/gcs/forwardPath", DronePath,
self.save_path) #Activity when new path is calculated
self.drone_type_sub = rospy.Subscriber("/gcs/medicalTransport",
DroneSingleValue,
self.update_type)
self.dnfz_pub = rospy.Publisher('/utm/dynamic_no_fly_zones',
String,
queue_size=10)
self.heartbeat_pub = rospy.Publisher('/node_monitor/input/Heartbeat',
heartbeat,
queue_size=10)
self.utm_drones_pub = rospy.Publisher('/utm/dronesList',
UTMDroneList,
queue_size=10)
self.heart_msg = heartbeat()
self.recent_drone = dict_init()
self.posted = False
self.last_posted_dnfz = self.get_dynamic_nfz()
self.dynamic_polys = []
self.dynamic_circles = []
self.static_zones = []
self.static_polys = []
def __init__(self):
self.request_sent = False
self.first_msg_ok = False
# status variables
self.batt_volt = 0.0
self.last_heard = 0
self.last_heard_sys_status = 0
self.lat = 0.0
self.lon = 0.0
self.alt = 0.0
self.local_lat = 0.0
self.local_lon = 0.0
self.local_alt = -5.0
self.local_pos = np.empty((1, 3), float)
self.recorded_sys_id = 0
self.recorded_comp_id = 0
self.utmconv = utm.utmconv()
self.target_lat = 55.4720459
self.target_lon = 10.4148503
(_, _, _, easting,
northing) = self.utmconv.geodetic_to_utm(self.target_lat,
self.target_lon)
self.target_easting = easting
self.target_northing = northing
self.mission_handler = mission_handler.MissionHandler()
self.current_mission_no = 0
self.last_mission_no = 0
self.command_handler = command_handler.CommandHandler()
rospy.sleep(1) # wait until everything is running
self.boot_time = rospy.Time.now().to_sec() * 1000
self.drone_boot_epoch = 0
# Service handlers
self.arm_service = rospy.Service("/telemetry/arm_drone",
Trigger,
self.command_handler.arm_drone,
buff_size=10)
self.disarm_service = rospy.Service("/telemetry/disarm_drone",
Trigger,
self.command_handler.disarm_drone,
buff_size=10)
self.takeoff_service = rospy.Service(
"/telemetry/takeoff_drone",
TakeoffDrone,
self.command_handler.takeoff_drone,
buff_size=10)
self.land_service = rospy.Service("/telemetry/land_drone",
LandDrone,
self.command_handler.land_drone,
buff_size=10)
self.start_mission_service = rospy.Service(
"/telemetry/start_mission",
Trigger,
self.command_handler.start_mission,
buff_size=10)
self.goto_waypoint = rospy.Service("/telemetry/goto_waypoint",
GotoWaypoint,
self.command_handler.goto_waypoint,
buff_size=10)
self.return_home_service = rospy.Service(
"/telemetry/return_home",
Trigger,
self.command_handler.return_home,
buff_size=10)
self.set_home_service = rospy.Service("/telemetry/set_home",
SetHome,
self.command_handler.set_home,
buff_size=10)
self.set_mode_service = rospy.Service("/telemetry/set_mode",
SetMode,
self.command_handler.set_mode,
buff_size=10)
self.guided_enable_service = rospy.Service(
"/telemetry/guided_enable",
Trigger,
self.command_handler.guided_enable,
buff_size=10)
self.guided_disable_service = rospy.Service(
"/telemetry/guided_disable",
Trigger,
self.command_handler.guided_disable,
buff_size=10)
self.change_speed_service = rospy.Service(
"/telemetry/change_speed",
ChangeSpeed,
self.command_handler.change_speed,
buff_size=10)
self.dowload_mission_service = rospy.Service(
"/telemetry/download_mission",
Trigger,
self.mission_handler.download,
buff_size=10)
self.mission_upload_service = rospy.Service(
"/telemetry/upload_mission",
UploadMission,
self.mission_handler.upload,
buff_size=10)
self.mission_upload_from_file_service = rospy.Service(
"/telemetry/upload_mission_from_file",
UploadFromFile,
self.mission_handler.upload_from_file,
buff_size=10)
self.start_tracking_service = rospy.Service(
"/telemetry/start_tracking",
Trigger,
self.start_tracking,
buff_size=10)
self.stop_tracking_service = rospy.Service("/telemetry/stop_tracking",
Trigger,
self.stop_tracking,
buff_size=10)
# Topic handlers
self.mavlink_msg_pub = rospy.Publisher(mavlink_lora_pub_topic,
mavlink_lora_msg,
queue_size=0)
self.statustext_pub = rospy.Publisher("/telemetry/statustext",
telemetry_statustext,
queue_size=0)
self.heartbeat_status_pub = rospy.Publisher(
"/telemetry/heartbeat_status",
telemetry_heartbeat_status,
queue_size=0)
self.mav_mode_pub = rospy.Publisher("/telemetry/mav_mode",
telemetry_mav_mode,
queue_size=0)
self.vfr_hud_pub = rospy.Publisher("/telemetry/vfr_hud",
telemetry_vfr_hud,
queue_size=0)
self.home_position_pub = rospy.Publisher("/telemetry/home_position",
telemetry_home_position,
queue_size=0)
self.control_commands_pub = rospy.Publisher(
"/telemetry/control_commands",
telemetry_control_commands,
queue_size=0)
self.imu_ned_pub = rospy.Publisher("/telemetry/imu_data_ned",
telemetry_imu_ned,
queue_size=0)
self.local_position_ned_pub = rospy.Publisher(
"/telemetry/local_position_ned",
telemetry_local_position_ned,
queue_size=0)
rospy.Subscriber(mavlink_lora_sub_topic, mavlink_lora_msg,
self.on_mavlink_msg)
rospy.Subscriber(mavlink_lora_pos_sub_topic, mavlink_lora_pos,
self.on_mavlink_lora_pos)
rospy.Subscriber(mavlink_lora_status_sub_topic, mavlink_lora_status,
self.on_mavlink_lora_status)
rospy.Subscriber("/telemetry/new_mission", mavlink_lora_mission_list,
self.mission_handler.on_mission_list)
rospy.Subscriber("/telemetry/mission_set_current", Int16,
self.mission_handler.mission_set_current)
rospy.Subscriber("/telemetry/set_landing_target",
telemetry_landing_target, self.on_landing_target)
rospy.Subscriber("/mavlink_interface/mission/ack",
mavlink_lora_mission_ack,
self.mission_handler.on_mission_ack)
rospy.Subscriber("/mavlink_interface/command/ack",
mavlink_lora_command_ack,
self.command_handler.on_command_ack)
self.rate = rospy.Rate(update_interval)
from utm import utmconv
from math import sqrt
def find_dist(pt1, pt2):
return sqrt((pt1[0]-pt2[0])**2+(pt1[1]-pt2[1])**2)
uc = utmconv()
pt1 = (55.371010, 10.427521)
pt2 = (55.500685, 9.764382)
(hemisphere1, zone1, letter1, e1, n1) = uc.geodetic_to_utm(pt1[0], pt1[1])
(hemisphere2, zone2, letter2, e2, n2) = uc.geodetic_to_utm(pt2[0], pt2[1])
pt1 = (e1, n1)
pt2 = (e2, n2)
print (find_dist(pt1, pt2))
def __init__(self):
self.count = 0
self.GEO_class = ecefGeo_class()
self.uc = utmconv() # <---- from Kjelds lib
self.t_prev = 0.0
self.t_current = 0.0
self.dt = 0.0
self.data = None
self.FirstRun_dt = True
self.Pub_data = False # <------ Remove ?
self.FirstRun_pos = True
self.FirstRun_pub = True
self.gps_vel = 0.0 # [m/s]
self.cog_gps = 0.0 #[cdeg]
self.cog_amount_steps = 5.0 # [cdeg]
self.cog_step_count = 0
self.cog_step_dir = (1.0871889239775937e-05) / 10000.0 #/10.0#1.0
self.cog_current = None
self.cog_drone_start = None
self.cog_drone_end = None
self.h = std_msgs.msg.Header()
#UTM spoofing
self.speed_x = 0.0 #[m/s]
self.speed_y = -1.5 #[m/s]
self.spoofing_distance = 50 # [m]
#Ground truth (GT)
self.gt_pos_x = 0
self.gt_pos_y = 0
self.runs = 0
self.load_data_first = True
#The data from the raw gps signal
self.gps_in_lat = 0.0
self.gps_in_lon = 0.0
self.gps_in_alt = 0.0
# The xyz for ecef
self.ecef_x = 0.0
self.ecef_y = 0.0
self.ecef_z = 0.0
#The output from utm
self.e1 = 0.0
self.n1 = 0.0
self.hemisphere = None
self.zone = None
# The spoofing inc offset
self.lat_inc = 0.0 # x
self.lon_inc = 0.0 # y
self.alt_inc = 0.0 # z
#The spoofing data that will be sent to the HIL_GPS
self.gps_out_lat = 0.0
self.gps_out_lon = 0.0
self.gps_out_alt = 0.0
#init the node and subscribe to the raw signal from the gps
rospy.init_node('GNSS_spoofing_wiht_GNSS_2_UTM_2_GNSS', anonymous=True)
#### -------- Check if mavros/global_position/raw/fix gives better results ----- ########
rospy.Subscriber(
'/mavros/global_position/global', NavSatFix, self.load_data
) #The '/mavros/global_position/global' used also the data from the IMU (http://wildfirewatch.elo.utfsm.cl/ros-topology/#global_gps)
rospy.Subscriber('/mavros/altitude', Altitude, self.load_height)
self.sub_path_check = rospy.Subscriber('/gazebo/model_states',
ModelStates, self.load_GT)
self.pub_spoofing_signal = rospy.Publisher(
'/fake_gps/offset_inc', GeoPoint,
queue_size=10) # This is for the spoofing part
self.change_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)
# The amount of time it should publish data
self.freq = 2.5
self.rosRate = rospy.Rate(self.freq) # 2.5 Hz
#rospy.spin()
while not rospy.is_shutdown():
#if self.load_data_first is False:
#self.transform_gps_xyz_ECEF(0.0) # This is zero because we know the start pos
#self.transform_gps_xyz_UTM(0) # This is zero because we know the start pos
if self.gt_pos_x >= self.spoofing_distance:
print('SPoofing!!')
self.publish_data()
self.rosRate.sleep()
self.load_data_first = False
else:
print('Not Spoofing!!')
self.rosRate.sleep()
