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, Name, latitude, longitude, altitude, isLab):
self.Name = Name
self.location = GPS()
self.location.latitude = latitude
self.location.longitude = longitude
self.location.altitude = altitude
self.status = 0
if isinstance(isLab, str):
self.isLab = (isLab == "True") or (isLab == "true")
else:
self.isLab = isLab
def publish_on_ros(self,
drone_id,
zone_type,
index1=-1,
index2=-1,
gps_start=None,
gps_end=None,
valid_to=0,
dnfz_id=-1):
if gps_start is None or gps_end is None:
gps_start = GPS()
gps_end = GPS()
msg = inCollision()
msg.drone_id = drone_id
msg.zone_type = zone_type
msg.plan_index1 = index1
msg.plan_index2 = index2
msg.start = gps_start
msg.end = gps_end
msg.valid_to = valid_to
msg.dnfz_id = int(dnfz_id)
self.collision_detected_pub.publish(msg)
def __init__(self, drone_id=1):
# All variables relating to the status and information of the drone
self.id = drone_id
self.up_time = 0
self.gps_timestamp = 0
self.fsm_state = State.GROUNDED
self.new_mission = False
self.upload_done = False
self.upload_failed = False
self.cmd_try_again = False
self.speed_ack = False
self.loiter = False
self.active = False
self.wait = False
self.hold = False
self.upload_retries = 0
self.mission_id = 0
self.pending_mission_gps = []
self.active_waypoint_gps = GPS()
self.active_mission_gps = []
self.active_mission_ml = mavlink_lora_mission_list()
self.active_mission_len = 0
self.active_mission_idx = 0
self.dist_to_goal = -1
self.holding_waypoint = GPS()
# the drone starts on the ground ( land means ground for now. Change later ) TODO
self.gcs_status = DroneInfo.Land
# from mission info
# self.active_sub_waypoint_idx = 0
# self.active_sub_mission_len = 0
# self.active_sub_waypoint = mavlink_lora_mission_item_int()
# from heartbeat status
self.main_mode = ""
self.sub_mode = ""
self.autopilot = ""
self.type = ""
self.state = ""
# from mav mode
self.armed = False
self.manual_input = False
self.hil_simulation = False
self.stabilized_mode = False
self.guided_mode = False
self.auto_mode = False
self.test_mode = False
self.custom_mode = False
# from statustext
self.statustext = deque([], maxlen=BUFFER_SIZE)
self.severity = deque([], maxlen=BUFFER_SIZE)
# from vfr hud
self.ground_speed = 0
self.climb_rate = 0
self.throttle = 0
# from home position
self.home_position = GPS()
# from drone attitude
self.roll = 0
self.pitch = 0
self.yaw = 0
# from drone status
self.battery_voltage = 0
self.battery_SOC = 0
self.cpu_load = 0
self.msg_sent_gcs = 0
self.msg_received_gcs = 0
self.msg_dropped_gcs = 0
self.msg_dropped_uas = 0
self.last_heard = rospy.Time().now()
# from drone pos
self.latitude = 0
self.longitude = 0
self.absolute_alt = 0
self.relative_alt = 0
self.heading = 0
self.clear_mission_pub = rospy.Publisher(
"/mavlink_interface/mission/mavlink_clear_all",
Empty,
queue_size=0)
self.upload_mission_pub = rospy.Publisher("/telemetry/new_mission",
mavlink_lora_mission_list,
queue_size=0)
self.set_current_mission_pub = rospy.Publisher(
"/telemetry/mission_set_current", Int16, queue_size=10)
# TODO add drone id to services
self.arm = rospy.ServiceProxy("/telemetry/arm_drone", Trigger)
self.disarm = rospy.ServiceProxy("/telemetry/disarm_drone", Trigger)
self.takeoff = rospy.ServiceProxy("/telemetry/takeoff_drone",
TakeoffDrone)
self.land = rospy.ServiceProxy("/telemetry/land_drone", LandDrone)
self.set_mode = rospy.ServiceProxy("/telemetry/set_mode", SetMode)
self.set_home = rospy.ServiceProxy("/telemetry/set_home", SetHome)
self.change_speed = rospy.ServiceProxy("/telemetry/change_speed",
ChangeSpeed)
self.return_to_home = rospy.ServiceProxy("/telemetry/return_home",
Trigger)
self.reposition = rospy.ServiceProxy("/telemetry/goto_waypoint",
GotoWaypoint)
self.upload_mission = rospy.ServiceProxy("/telemetry/upload_mission",
UploadMission)
# class that handles the missions manually while an upload is in progress
self.manual_mission = manual_mission.ManualMission(
target_sys=self.id,
target_comp=0,
reposition_handle=self.reposition)
def send_info_cb(self, event):
# TODO iterate over all drones
drone = self.drones[1]
if drone.active:
try:
need2know = DroneInfo(
drone_id=drone.id,
position=GPS(drone.latitude, drone.longitude, drone.relative_alt),
next_waypoint=drone.active_waypoint_gps,
armed=drone.armed,
ground_speed=drone.ground_speed,
ground_speed_setpoint=5,
heading=drone.heading,
battery_voltage=drone.battery_voltage,
battery_SOC=drone.battery_SOC,
relative_alt=drone.relative_alt,
absolute_alt=drone.absolute_alt,
GPS_timestamp=drone.gps_timestamp,
status=drone.gcs_status,
path_id=drone.mission_id,
mission_index=drone.active_mission_idx,
mission_length=drone.active_mission_len
)
nice2know = NiceInfo(
drone_id=drone.id,
drone_handler_state=str(drone.fsm_state),
last_heard=drone.last_heard,
up_time=int(drone.up_time),
RPY=[drone.roll, drone.pitch, drone.yaw],
main_flightmode=drone.main_mode,
sub_flightmode=drone.sub_mode,
msg_sent_gcs=drone.msg_sent_gcs,
msg_received_gcs=drone.msg_received_gcs,
msg_dropped_gcs=drone.msg_dropped_gcs,
msg_dropped_uas=drone.msg_dropped_uas,
active_waypoint_idx=drone.active_mission_idx,
active_mission_len=drone.active_mission_len,
armed=drone.armed,
manual_input=drone.manual_input,
hil_simulation=drone.hil_simulation,
stabilized_mode=drone.stabilized_mode,
guided_mode=drone.guided_mode,
auto_mode=drone.auto_mode,
test_mode=drone.test_mode,
custom_mode=drone.custom_mode,
autopilot=drone.autopilot,
mav_state=drone.state,
mav_type=drone.type,
climb_rate=drone.climb_rate,
throttle=drone.throttle,
cpu_load=drone.cpu_load,
home=drone.home_position
)
self.drone_info_pub.publish(need2know)
self.nice_info_pub.publish(nice2know)
except Exception as e:
print(e)
def on_home_position(self, msg):
if msg.system_id in self.drones:
drone = self.drones[msg.system_id]
drone.home_position = GPS(msg.latitude, msg.longitude, msg.altitude)