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)