|
| 1 | +""" |
| 2 | +Launch multiple copters in Gazebo |
| 3 | +
|
| 4 | +Assume Gazebo Jetty (no namespace required for the imports) |
| 5 | +""" |
| 6 | + |
| 7 | +import copy |
| 8 | +import math |
| 9 | +import os |
| 10 | + |
| 11 | +from argparse import ArgumentParser |
| 12 | +from pathlib import Path |
| 13 | +from transforms3d import euler |
| 14 | + |
| 15 | +GZ_VERSION_GARDEN = "garden" |
| 16 | +GZ_VERSION_HARMONIC = "harmonic" |
| 17 | +GZ_VERSION_IONIC = "ionic" |
| 18 | +GZ_VERSION_JETTY = "jetty" |
| 19 | + |
| 20 | +# TODO: hardcoded path - need to search the GZ_RESOURCE_PATH |
| 21 | +GZ_RESOURCE_PATH = "/Users/rhys/Code/ros2/jazzy/ros2-ardupilot/src/ardupilot_gazebo/models" |
| 22 | + |
| 23 | +# Index convention for transforms3d quaternions |
| 24 | +QUAT_IDX_W = 0 |
| 25 | +QUAT_IDX_X = 1 |
| 26 | +QUAT_IDX_Y = 2 |
| 27 | +QUAT_IDX_Z = 3 |
| 28 | + |
| 29 | + |
| 30 | +def gz_version(): |
| 31 | + """Return the environment variable GZ_VERSION if set, else default to 'harmonic'""" |
| 32 | + return os.environ.get("GZ_VERSION", GZ_VERSION_HARMONIC) |
| 33 | + |
| 34 | + |
| 35 | +if gz_version() == GZ_VERSION_JETTY: |
| 36 | + from gz.msgs.boolean_pb2 import Boolean |
| 37 | + from gz.msgs.entity_factory_pb2 import EntityFactory |
| 38 | + from gz.msgs.entity_factory_v_pb2 import EntityFactory_V |
| 39 | + |
| 40 | + |
| 41 | +# Importing gz.transport into the module global scope causes an odd |
| 42 | +# multiprocessing conflict with dronecan. This is a workaround. |
| 43 | +def gz_node(): |
| 44 | + if gz_version() == GZ_VERSION_JETTY: |
| 45 | + from gz.transport import Node |
| 46 | + |
| 47 | + return Node() |
| 48 | + |
| 49 | +class GazeboModelFactory: |
| 50 | + def __init__(self, args): |
| 51 | + # Configuration |
| 52 | + self.world_name = args.world |
| 53 | + self.model_name = args.model |
| 54 | + |
| 55 | + # The sysid and instance may not be coincident |
| 56 | + self.first_sysid = args.first_sysid |
| 57 | + self.first_instance = args.first_instance |
| 58 | + |
| 59 | + # Layout |
| 60 | + self.row_spacing = args.row_spacing |
| 61 | + self.col_spacing = args.col_spacing |
| 62 | + self.z_offset = args.z_offset |
| 63 | + self.row_count = args.row_count |
| 64 | + self.col_count = args.col_count |
| 65 | + |
| 66 | + # Service call timeout |
| 67 | + self.timeout = args.timeout |
| 68 | + |
| 69 | + # TODO: hardcoded path - need to search the GZ_RESOURCE_PATH |
| 70 | + # Load the sdf file |
| 71 | + self.path_to_models = Path(GZ_RESOURCE_PATH) |
| 72 | + |
| 73 | + def create_models(self): |
| 74 | + # Configuration |
| 75 | + world_name = self.world_name |
| 76 | + model_name = self.model_name |
| 77 | + first_sysid = self.first_sysid |
| 78 | + first_instance = self.first_instance |
| 79 | + row_spacing = self.row_spacing |
| 80 | + col_spacing = self.col_spacing |
| 81 | + z_offset = self.z_offset |
| 82 | + row_count = self.row_count |
| 83 | + col_count = self.col_count |
| 84 | + timeout = self.timeout |
| 85 | + path_to_models = self.path_to_models |
| 86 | + |
| 87 | + model_sdf = None |
| 88 | + with open(path_to_models / model_name / "model.sdf", mode="r") as f: |
| 89 | + model_sdf = f.read() |
| 90 | + # print(model_sdf) |
| 91 | + |
| 92 | + # Set up the service call |
| 93 | + node = gz_node() |
| 94 | + service_name = f"/world/{world_name}/create_multiple" |
| 95 | + |
| 96 | + # Create all models in a single request |
| 97 | + request = EntityFactory_V() |
| 98 | + |
| 99 | + # Set global orientation and initial sysid and instance |
| 100 | + q = euler.euler2quat(math.radians(0), math.radians(0), math.radians(90)) |
| 101 | + sysid = first_sysid |
| 102 | + instance = first_instance |
| 103 | + for row in range(row_count): |
| 104 | + for col in range(col_count): |
| 105 | + # Replace address and port details in sdf |
| 106 | + fdm_addr = "127.0.0.1" |
| 107 | + fdm_port_in = 9002 + 10 * instance |
| 108 | + |
| 109 | + instance_model_sdf = model_sdf.replace( |
| 110 | + f"<fdm_addr>127.0.0.1</fdm_addr>", f"<fdm_addr>{fdm_addr}</fdm_addr>" |
| 111 | + ) |
| 112 | + |
| 113 | + instance_model_sdf = instance_model_sdf.replace( |
| 114 | + f"<fdm_port_in>9002</fdm_port_in>", |
| 115 | + f"<fdm_port_in>{fdm_port_in}</fdm_port_in>", |
| 116 | + ) |
| 117 | + |
| 118 | + # Create entity |
| 119 | + entity_factory = EntityFactory() |
| 120 | + entity_factory.sdf = instance_model_sdf |
| 121 | + entity_factory.pose.position.x = row_spacing * row |
| 122 | + entity_factory.pose.position.y = col_spacing * col |
| 123 | + entity_factory.pose.position.z = z_offset |
| 124 | + |
| 125 | + entity_factory.pose.orientation.w = q[QUAT_IDX_W] |
| 126 | + entity_factory.pose.orientation.x = q[QUAT_IDX_X] |
| 127 | + entity_factory.pose.orientation.y = q[QUAT_IDX_Y] |
| 128 | + entity_factory.pose.orientation.z = q[QUAT_IDX_Z] |
| 129 | + |
| 130 | + entity_factory.name = f"{model_name}_{sysid}" |
| 131 | + entity_factory.allow_renaming = False |
| 132 | + |
| 133 | + # Append to request |
| 134 | + request.data.append(entity_factory) |
| 135 | + |
| 136 | + sysid += 1 |
| 137 | + instance += 1 |
| 138 | + |
| 139 | + response = Boolean() |
| 140 | + |
| 141 | + print(f"Creating models: {model_name} x {row_count * col_count}") |
| 142 | + result, response = node.request( |
| 143 | + service_name, request, EntityFactory_V, Boolean, timeout |
| 144 | + ) |
| 145 | + print(f"Creating models: result: {result}, response: {response.data}") |
| 146 | + |
| 147 | + |
| 148 | +class SitlLauncher: |
| 149 | + def __init__(self): |
| 150 | + pass |
| 151 | + |
| 152 | + def launch(self): |
| 153 | + pass |
| 154 | + |
| 155 | + |
| 156 | +def main(): |
| 157 | + # Command line args |
| 158 | + parser = ArgumentParser(description="Launch Copter Swarm") |
| 159 | + parser.add_argument("--world", default="runway", type=str, help="world name") |
| 160 | + parser.add_argument( |
| 161 | + "--model", default="iris_with_ardupilot", type=str, help="model name" |
| 162 | + ) |
| 163 | + |
| 164 | + parser.add_argument("--row-count", default="1", type=int, help="number of rows") |
| 165 | + parser.add_argument("--col-count", default="1", type=int, help="number of columns") |
| 166 | + parser.add_argument( |
| 167 | + "--row-spacing", |
| 168 | + default="1.0", |
| 169 | + type=float, |
| 170 | + help="spacing between copters along a row", |
| 171 | + ) |
| 172 | + parser.add_argument( |
| 173 | + "--col-spacing", |
| 174 | + default="1.0", |
| 175 | + type=float, |
| 176 | + help="spacing between copters along a column", |
| 177 | + ) |
| 178 | + parser.add_argument( |
| 179 | + "--z-offset", |
| 180 | + default="1.0", |
| 181 | + type=float, |
| 182 | + help="spacing between copters along a column", |
| 183 | + ) |
| 184 | + parser.add_argument( |
| 185 | + "--timeout", default="5000", type=int, help="timeout for service calls" |
| 186 | + ) |
| 187 | + parser.add_argument( |
| 188 | + "--first-sysid", default="1", type=int, help="sysid of the first copter" |
| 189 | + ) |
| 190 | + parser.add_argument( |
| 191 | + "--first-instance", |
| 192 | + default="0", |
| 193 | + type=int, |
| 194 | + help="instance index of the first copter", |
| 195 | + ) |
| 196 | + |
| 197 | + args = parser.parse_args() |
| 198 | + |
| 199 | + # ======================================================================= # |
| 200 | + # Create copter models |
| 201 | + |
| 202 | + gz_model_factory = GazeboModelFactory(args) |
| 203 | + |
| 204 | + # overrides for testing |
| 205 | + gz_model_factory.row_count = 2 |
| 206 | + gz_model_factory.col_count = 2 |
| 207 | + |
| 208 | + gz_model_factory.create_models() |
| 209 | + |
| 210 | + # TODO |
| 211 | + # Add levels |
| 212 | + # Add transparent cells / tiles to highlight level boundaries |
| 213 | + # Add performers |
| 214 | + |
| 215 | + # ======================================================================= # |
| 216 | + # Launch SITL |
| 217 | + |
| 218 | + # Single session to start... |
| 219 | + sitl_launcher = SitlLauncher() |
| 220 | + sitl_launcher.launch() |
| 221 | + |
| 222 | + # ======================================================================= # |
| 223 | + |
| 224 | + |
| 225 | +if __name__ == "__main__": |
| 226 | + main() |
0 commit comments