def _read_waypoints_pb_110(self, file):
if not HAVE_PROTOBUF:
raise MAVWPError(
'Cannot read mission file in protobuf format without protobuf '
'library. Try "easy_install protobuf".')
explicit_seq = False
warned_seq = False
mission = mission_pb2.Mission()
text_format.Merge(file.read(), mission)
defaults = mission_pb2.Waypoint()
# Set defaults (may be overriden in file).
defaults.current = False
defaults.autocontinue = True
defaults.param1 = 0.0
defaults.param2 = 0.0
defaults.param3 = 0.0
defaults.param4 = 0.0
defaults.x = 0.0
defaults.y = 0.0
defaults.z = 0.0
# Use defaults specified in mission file, if there are any.
if mission.defaults:
defaults.MergeFrom(mission.defaults)
for seq, waypoint in enumerate(mission.waypoint):
# Consecutive sequence numbers are automatically assigned
# UNLESS the mission file specifies sequence numbers of
# its own.
if waypoint.seq:
explicit_seq = True
else:
if explicit_seq and not warned_seq:
logging.warn(
'Waypoint file %s: mixes explicit and implicit '
'sequence numbers' % (file,))
warned_seq = True
# The first command has current=True, the rest have current=False.
if seq > 0:
current = defaults.current
else:
current = True
w = mavutil.mavlink.MAVLink_mission_item_message(
self.target_system, self.target_component,
waypoint.seq or seq,
waypoint.frame,
waypoint.command,
waypoint.current or current,
waypoint.autocontinue or defaults.autocontinue,
waypoint.param1 or defaults.param1,
waypoint.param2 or defaults.param2,
waypoint.param3 or defaults.param3,
waypoint.param4 or defaults.param4,
waypoint.x or defaults.x,
waypoint.y or defaults.y,
waypoint.z or defaults.z)
self.add(w)
def save_as_pb(self, filename):
mission = mission_pb2.Mission()
for w in self.wpoints:
waypoint = mission.waypoint.add()
waypoint.command = w.command
waypoint.frame = w.frame
waypoint.seq = w.seq
waypoint.current = w.current
waypoint.autocontinue = w.autocontinue
waypoint.param1 = w.param1
waypoint.param2 = w.param2
waypoint.param3 = w.param3
waypoint.param4 = w.param4
waypoint.x = w.x
waypoint.y = w.y
waypoint.z = w.z
with open(filename, 'w') as f:
f.write('QGC WPL PB 110\n')
f.write(text_format.MessageToString(mission))
def run():
global thread_status
channel = grpc.insecure_channel('0.0.0.0:50051')
# stub = dronecore_pb2_grpc.DroneCoreRPCStub(channel)
action_stub = action_pb2_grpc.ActionRPCStub(channel)
mission_stub = mission_pb2_grpc.MissionRPCStub(channel)
mission_items = []
mission_items.append(
dc_mission.MissionItem(latitude_deg=47.398170327054473,
longitude_deg=8.5456490218639658,
relative_altitude_m=10,
speed_m_s=5,
is_fly_through=False,
gimbal_pitch_deg=20,
gimbal_yaw_deg=60,
camera_action=dc_mission.MissionItem.NONE))
mission_items.append(
dc_mission.MissionItem(
latitude_deg=47.398241338125118,
longitude_deg=8.5455360114574432,
relative_altitude_m=10,
speed_m_s=2,
is_fly_through=True,
gimbal_pitch_deg=0,
gimbal_yaw_deg=-60,
camera_action=dc_mission.MissionItem.TAKE_PHOTO))
mission_items.append(
dc_mission.MissionItem(
latitude_deg=47.398139363821485,
longitude_deg=8.5453846156597137,
relative_altitude_m=10,
speed_m_s=5,
is_fly_through=True,
gimbal_pitch_deg=-45,
gimbal_yaw_deg=0,
camera_action=dc_mission.MissionItem.START_VIDEO))
mission_items.append(
dc_mission.MissionItem(
latitude_deg=47.398058617228855,
longitude_deg=8.5454618036746979,
relative_altitude_m=10,
speed_m_s=2,
is_fly_through=False,
gimbal_pitch_deg=-90,
gimbal_yaw_deg=30,
camera_action=dc_mission.MissionItem.STOP_VIDEO))
mission_items.append(
dc_mission.MissionItem(
latitude_deg=47.398100366082858,
longitude_deg=8.5456969141960144,
relative_altitude_m=10,
speed_m_s=5,
is_fly_through=False,
gimbal_pitch_deg=-45,
gimbal_yaw_deg=-30,
camera_action=dc_mission.MissionItem.START_PHOTO_INTERVAL))
mission_items.append(
dc_mission.MissionItem(
latitude_deg=47.398001890458097,
longitude_deg=8.5455576181411743,
relative_altitude_m=10,
speed_m_s=5,
is_fly_through=False,
gimbal_pitch_deg=0,
gimbal_yaw_deg=0,
camera_action=dc_mission.MissionItem.STOP_PHOTO_INTERVAL))
mission_stub.SendMission(dc_mission.Mission(mission_items=mission_items))
time.sleep(1)
action_stub.Arm(empty_pb2.Empty())
time.sleep(1)
future_status = mission_stub.StartMission.future(empty_pb2.Empty())
t = threading.Thread(target=wait_func, args=(future_status, ))
t.start()
while (thread_status):
print("Waiting for thread to exit")
time.sleep(5)
