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()