def launch_firmware(self, ID, uav_roslaunch_args): rinfo('Running firmware for uav' + str(ID) + '...') uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) roslaunch_sequence = [(self.path_to_launch_file_firmware, uav_roslaunch_args)] launch = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_sequence) try: launch.start() except: rerr('Error occured while starting firmware for uav' + str(ID) + '!') launch.shutdown() return None try: rospy.wait_for_message('/uav' + str(ID) + '/mavros/state', MavrosState, 60) except: rerr('Mavros did not respond while starting firmware for uav' + str(ID) + '!') launch.shutdown() return None rinfo('Firmware for uav' + str(ID) + ' started!') return launch
def get_spawn_poses_from_file(self, filename, uav_ids): if not os.path.isfile(filename): raise Exception('File \'' + str(filename) + '\' does not exist!') spawn_poses = {} # #{ csv if filename.endswith('.csv'): array_string = list(csv.reader(open(filename))) for row in array_string: if (len(row) != 5): raise Exception( 'Incorrect data in file \'' + str(filename) + '\'! Data in \'.csv\' file type should be in format [id, x, y, z, heading] (example: int, float, float, float, float)' ) if int(row[0]) in uav_ids: spawn_poses[int(row[0])] = { 'x': float(row[1]), 'y': float(row[2]), 'z': float(row[3]), 'heading': float(row[4]) } # #} # #{ yaml elif filename.endswith('.yaml'): dict_vehicle_info = yaml.safe_load(open(filename, 'r')) for item, data in dict_vehicle_info.items(): if (len(data.keys()) != 5): raise Exception( 'Incorrect data in file \'' + str(filename) + '\'! Data in \'.yaml\' file type should be in format \n uav_name: \n\t id: (int) \n\t x: (float) \n\t y: (float) \n\t z: (float) \n\t heading: (float)' ) if int(data['id']) in uav_ids: spawn_poses[data['id']] = { 'x': float(data['x']), 'y': float(data['y']), 'z': float(data['z']), 'heading': float(data['heading']) } # #} else: raise Exception( 'Incorrect file format, must be either \'.csv\' or \'.yaml\'') if len(spawn_poses.keys()) != len(uav_ids) or set( spawn_poses.keys()) != set(uav_ids): raise Exception('File \'' + str(filename) + '\' does not cover all the UAV poses') rinfo('Spawn poses returned:') rinfo(str(spawn_poses)) return spawn_poses
def spawn_simulation_model(self, ID, uav_roslaunch_args): rinfo('Spawning Gazebo model for uav' + str(ID) + '...') uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) roslaunch_sequence = [(self.path_to_launch_file_spawn_model, uav_roslaunch_args)] launch = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_sequence) try: launch.start() except: rerr('Error occured while spawning Gazebo model for uav' + str(ID) + '!') launch.shutdown() return None rinfo('Gazebo model for uav' + str(ID) + ' spawned!') self.queued_vehicles.remove('uav' + str(ID)) self.active_vehicles.append('uav' + str(ID)) return launch
def launch_mavros(self, ID, uav_roslaunch_args): rinfo('Running mavros for uav' + str(ID) + '...') uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) roslaunch_sequence = [(self.path_to_launch_file_mavros, uav_roslaunch_args)] launch = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_sequence) try: launch.start() except: rerr('Error occured while launching mavros for uav' + str(ID) + '!') launch.shutdown() return None rinfo('Waiting for uav' + str(ID) + '\'s mavros to start publishing...') try: rospy.wait_for_message('/uav' + str(ID) + '/mavros/state', MavrosState, 60) except: rerr('Mavros for uav' + str(ID) + ' did not start in 60 seconds!') launch.shutdown() return None rinfo('Mavros for uav' + str(ID) + ' started!') return launch
def get_params_dict(self, params_list, vehicle_type): params_dict = copy.deepcopy(self.default_model_config) custom_params = {} for i, p in enumerate(params_list): if '--' in p: param_name = p[2:] param_name = param_name.replace('-', '_') if param_name not in self.default_model_config.keys( ) and param_name not in VEHICLE_TYPES: raise Exception('Param \'' + str(param_name) + '\' not recognized!') children = [] for j in range(i + 1, len(params_list)): if '--' not in params_list[j]: children.append(params_list[j]) else: break if len(children) < 1: custom_params[param_name] = True elif len(children) == 1: custom_params[param_name] = children[0] else: custom_params[param_name] = children if len(custom_params.keys()) > 0: rinfo('Customized params:') for pname, pval in custom_params.items(): if pname not in VEHICLE_TYPES: # check if the customized param is allowed for the desired vehicle type allowed_vehicle_types = self.spawner_params[pname][2] # print('For param ' + str(pname) + ' allowed: ' + str(allowed_vehicle_types)) if vehicle_type not in allowed_vehicle_types: raise Exception( 'Param \'' + str(pname) + '\' cannot be used with vehicle type \'' + str(vehicle_type) + '\'!') print('\t' + str(pname) + ': ' + str(pval)) params_dict.update(custom_params) return params_dict
def parse_input_params(self, data): params_list = data.split() try: uav_ids = self.get_ids(params_list) vehicle_type = self.get_vehicle_type(params_list) params_dict = self.get_params_dict(params_list, vehicle_type) if params_dict['pos_file'] != 'None': rinfo('Loading spawn poses from file \'' + str(params_dict['pos_file']) + '\'') spawn_poses = self.get_spawn_poses_from_file( params_dict['pos_file'], uav_ids) elif params_dict['pos'] != 'None': rinfo('Using spawn poses provided from command line args \'' + str(params_dict['pos']) + '\'') spawn_poses = self.get_spawn_poses_from_args( params_dict['pos'], uav_ids) else: rinfo('Assigning default spawn poses') spawn_poses = self.get_spawn_poses_from_ids(uav_ids) except Exception as e: rerr('Exception raised while parsing user input:') rerr(str(e.args[0])) raise Exception('Cannot spawn vehicle. Reason: ' + str(e.args[0])) params_dict['uav_ids'] = uav_ids params_dict['vehicle_type'] = vehicle_type params_dict['spawn_poses'] = spawn_poses return params_dict
def get_ids(self, params_list): requested_ids = [] # read params until non-numbers start comming for p in params_list: if is_number(p): requested_ids.append(int(p)) else: break if len(requested_ids) < 1: free_id = self.assign_free_id() requested_ids.append(free_id) rwarn('Vehicle ID not specified. Number ' + str(free_id) + ' assigned automatically.') return requested_ids rinfo('Requested vehicle IDs: ' + str(requested_ids)) ids = [] # remove all IDs that are already assigned or out of range for ID in requested_ids: if ID > 249: rwarn('Cannot spawn uav' + str(ID) + ', ID out of range <0, 250>!') continue if ID in self.assigned_ids.keys(): rwarn('Cannot spawn uav' + str(ID) + ', ID already assigned!') continue ids.append(ID) if len(ids) < 1: raise Exception('No valid ID provided') return ids
def __init__(self, show_help=False, verbose=False): self.verbose = verbose self.rospack = rospkg.RosPack() pkg_path = self.rospack.get_path('mrs_simulation') path_to_spawner_params = pkg_path + os.sep + 'config' + os.sep + 'spawner_params.yaml' with open(path_to_spawner_params, 'r') as params_file: self.spawner_params = yaml.safe_load(params_file) if not self.params_integrity_ok(): return if show_help: self.print_help() return # convert spawner_params to default_model_config (to get rid of help strings) self.default_model_config = {} for key, values in self.spawner_params.items(): self.default_model_config[key] = values[0] self.path_to_launch_file_firmware = pkg_path + os.sep + 'launch' + os.sep + 'run_simulation_firmware.launch' self.path_to_launch_file_spawn_model = pkg_path + os.sep + 'launch' + os.sep + 'spawn_simulation_model.launch' self.path_to_launch_file_mavros = pkg_path + os.sep + 'launch' + os.sep + 'run_simulation_mavros.launch' rospy.init_node('mrs_drone_spawner', anonymous=True) rinfo('Node initialization started. All parameters loaded correctly.') if self.verbose: rinfo('Loaded the following params:') for param, value in self.spawner_params.items(): print('\t\t' + str(param) + ': ' + str(value)) print('') rinfo( 'remove arg \'verbose\' in mrs_drone_spawner.launch to stop listing params on startup' ) # #{ setup system variables self.spawn_called = False self.processing = False self.process_queue = [] self.process_queue_mutex = threading.Lock() self.active_vehicles = [] self.queued_vehicles = [] self.got_mavlink = {} self.mavlink_sub = {} self.running_processes = [] self.assigned_ids = {} # dictionary {id: process_handle} # #} # #{ setup communication # diagnostics self.diagnostics_pub = rospy.Publisher('~diagnostics', SpawnerDiagnostics, queue_size=1) self.diagnostics_timer = rospy.Timer(rospy.Duration(0.1), self.callback_diagnostics_timer) self.action_timer = rospy.Timer(rospy.Duration(0.1), self.callback_action_timer) # spawning spawn_server = rospy.Service('~spawn', StringSrv, self.callback_spawn, buff_size=20) rospy.spin()