diff --git a/docs/conf.py b/docs/conf.py index 4ed77f2..c284e20 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -18,3 +18,7 @@ extensions = ['sphinx_honu'] honu_github = ('HonuRobotics', 'holybro_models') + +# Anchors for headings down to ###, so a page can link to a section +# (`page.md#section-title`) in a way GitHub renders too. +myst_heading_anchors = 3 diff --git a/docs/getting-started/first-simulation.md b/docs/getting-started/first-simulation.md index 36296d6..b184a0e 100644 --- a/docs/getting-started/first-simulation.md +++ b/docs/getting-started/first-simulation.md @@ -17,7 +17,7 @@ Gazebo opens with the X500 resting on its landing gear: - the GPS mast That is the default configuration; no configuration was involved. The -model carries no control layer, so the rotors idle until an autopilot +model carries no control layer, so the rotors idle until something publishes on the motor bus ([Actuators](../vehicles/x500/actuators.md)). diff --git a/docs/how-to/index.md b/docs/how-to/index.md index ef6b468..b157e0a 100644 --- a/docs/how-to/index.md +++ b/docs/how-to/index.md @@ -54,9 +54,62 @@ ros2 run x500_gazebo configure_vehicle.py --config $HOME/my_x500/my_loadout.yaml ``` That writes `x500.urdf`, `model.sdf`, `model.config`, -`ros_gz_bridge.yaml` and `robot_description.yaml`. Because the directory -holds a `model.sdf` next to a `model.config`, it works as a `model://` -root on `GZ_SIM_RESOURCE_PATH`. +`ros_gz_bridge.yaml` and `robot_description.yaml`, plus `vehicle.yaml`, +the config as used. Because the directory holds a `model.sdf` next to a +`model.config`, it works as a `model://` root on `GZ_SIM_RESOURCE_PATH`. +Add `--name ` to generate an instance under another name. + +## Several vehicles + +Every instance of the vehicle goes by one name, which is at once its +Gazebo model name, its topic namespace (`//...` on both the Gazebo +and the ROS side, joint states included) and its TF prefix (`/` +in front of every frame and every sensor `frame_id`). A name is letters, +digits and underscores, starting with a letter; the generator refuses +anything else. + +The launch argument is the name. `sim.launch.xml` always passes `name:=`, +`x500` by default, to `configure_vehicle.py --name`, which applies it to the +config before generating the artifacts, so the model name, the plugin +topics, the frames and the bridge cannot disagree. A `topic_namespace` in a +custom config is therefore overridden by the launch; it only names the +instance when the generator runs without `--name`. `x`, `y`, `z`, `roll`, +`pitch` and `yaw` place the instance: + +```bash +ros2 launch x500_gazebo sim.launch.xml name:=uav_b x:=2 yaw:=1.57 +``` + +Per part `topic`, `gz_topic` and `ros_topic` overrides go under the +instance name too, so two instances of one config never share a topic; an +override that starts with a slash is used as given and is then shared. +`extra_bridge_topics` are appended to the bridge config verbatim, so they +are shared by every instance too. + +`sim.launch.xml` starts a Gazebo server of its own, so running it twice +gives two simulations, not two quads. Several instances share one world +through gz-maritime, whose simulation launch names no vehicle and whose +spawn launch adds any vehicle by name and pose, running this generator for +it: + +```bash +ros2 launch kai_bringup simulation.launch.xml world:=$(ros2 pkg prefix --share x500_gazebo)/worlds/x500_ground.sdf +``` + +then, one command per quad: + +```bash +x500="$(ros2 pkg prefix x500_gazebo)/lib/x500_gazebo/configure_vehicle.py --config $(ros2 pkg prefix --share x500_description)/config/x500.yaml" +ros2 launch kai_bringup spawn_vehicle.launch.xml name:=uav_a y:=2 z:=0.25 generator:="$x500" +ros2 launch kai_bringup spawn_vehicle.launch.xml name:=uav_b y:=-2 z:=0.25 generator:="$x500" +``` + +Each quad gets its topics under its name, its own bridge and +`robot_state_publisher` in its namespace and its own TF prefix; the clock +is bridged once by the simulation launch, which drops the `/clock` entry +this bridge config carries. To run several **simulations** side by side +instead, give each Gazebo instance its own `GZ_PARTITION` and each ROS +graph its own `ROS_DOMAIN_ID`. To confirm a config and an assembled URDF still agree: diff --git a/docs/reference/index.md b/docs/reference/index.md index 8e73a6d..4cedb7d 100644 --- a/docs/reference/index.md +++ b/docs/reference/index.md @@ -66,9 +66,11 @@ half assembled vehicle. ## Topics See the X500 [Sensors](../vehicles/x500/sensors.md) and -[Actuators](../vehicles/x500/actuators.md) pages. `/clock` and -`/joint_states` are always bridged; every part topic follows -`///` on both sides. +[Actuators](../vehicles/x500/actuators.md) pages. `` is the +instance name. `/clock`, `//joint_states` and +`//command/motor_speed` are always bridged; every part topic follows +`///` on both sides, and a per part override goes +under the name unless it starts with a slash. ## Packages diff --git a/docs/vehicles/x500/actuators.md b/docs/vehicles/x500/actuators.md index 964ed7c..c117343 100644 --- a/docs/vehicles/x500/actuators.md +++ b/docs/vehicles/x500/actuators.md @@ -14,22 +14,45 @@ following the [Add a part](../../how-to/index.md) guide. ## Actuators ROS API -There is none, deliberately. On a multicopter the per rotor outputs only -make sense downstream of an attitude controller, so the autopilot (PX4 -SITL, ArduPilot SITL or a custom controller) is the control layer and it -talks to the Gazebo motor bus directly. The marine vehicles (BlueBoat, -BlueROV2) expose a normalized -1..1 `throttle` topic per thruster, the -ArduPilot output convention; the X500's equivalent normalized layer is -the autopilot itself. Rotor joint states are bridged to `/joint_states` -so RViz animates the props. +The four rotors are driven together over one motor command bus: + +| ROS Topic | Description | Message type | +|---|---|---| +| `//command/motor_speed` | Angular velocity per rotor (rad/s), one entry per rotor number, indexed from 0 | [actuator_msgs/msg/Actuators](https://github.com/rudislabs/actuator_msgs/blob/main/msg/Actuators.msg) | + +`` is the instance name, `x500` for the default instance, which the +commands below use, or whatever the quad was spawned as. + +The `velocity` array carries one angular velocity per rotor, in rad/s, +where entry `i` drives `rotor_i`. Each motor plugin ramps its rotor to the +commanded speed with the part's time constants and holds it until the next +message, so always send all four; a stopped rotor is a `0`. To spin all +four by hand: + +```bash +ros2 topic pub --once /x500/command/motor_speed actuator_msgs/msg/Actuators "{velocity: [700, 700, 700, 700]}" +``` + +and to stop them: + +```bash +ros2 topic pub --once /x500/command/motor_speed actuator_msgs/msg/Actuators "{velocity: [0, 0, 0, 0]}" +``` + +The bus is a low level interface: it sets rotor speeds and nothing more, +and keeping the quad level and steering it is the job of a controller +above it. Rotor joint +states are bridged to `//joint_states` so RViz animates the props. ## Gazebo transport API | gz Topic | Description | Message type | |---|---|---| -| `/x500/command/motor_speed` | Angular velocity per rotor (rad/s), indexed by the rotor number | `gz.msgs.Actuators` | +| `//command/motor_speed` | The same bus on the Gazebo side | `gz.msgs.Actuators` | -To spin the rotors by hand, with no autopilot: +The motor plugins build the topic from the instance name, so a quad spawned +as `uav_b` listens on `/uav_b/command/motor_speed`. The bridge maps the ROS +messages above onto it; the same command can be sent from the Gazebo side: ```bash gz topic -t /x500/command/motor_speed -m gz.msgs.Actuators -p 'velocity: [700, 700, 700, 700]' diff --git a/docs/vehicles/x500/index.md b/docs/vehicles/x500/index.md index 0a307d0..d54229b 100644 --- a/docs/vehicles/x500/index.md +++ b/docs/vehicles/x500/index.md @@ -21,8 +21,7 @@ Launch the X500 without a config and you get the airframe with: - **the 4S battery**. - **the GPS mast**. -That is enough to fly once an autopilot is attached or you provide -control code. Two more parts sit in the catalog unfitted, the companion +That is enough to fly once a controller drives the motor bus. Two more parts sit in the catalog unfitted, the companion computer and the gimbal camera, and nothing above is fixed: fit an optional part, swap a default for another accepted type, or leave a default off entirely. See [Configuration](configuration.md). diff --git a/docs/vehicles/x500/running.md b/docs/vehicles/x500/running.md index cae9100..d1b06c6 100644 --- a/docs/vehicles/x500/running.md +++ b/docs/vehicles/x500/running.md @@ -8,8 +8,8 @@ Launches the vehicle in its default configuration: every artifact of the loadout is generated at start (URDF, composed model, bridge config, into a directory under `$ROS_HOME`), the model is spawned as `x500` into the ground world and the ROS bridge comes up with it. The vehicle rests on -its landing gear; the rotors idle until an autopilot publishes on the -motor bus ([Actuators](actuators.md)). +its landing gear; the rotors idle until something publishes on the motor +bus ([Actuators](actuators.md)). ![](../../img/x500.png) @@ -17,6 +17,16 @@ To run a custom vehicle instead, pass a loadout file with `config_file:=`; the [configuration page](configuration.md) lists the slots and [Change the loadout](../../how-to/index.md) walks through writing one. +| Argument | Default | Meaning | +|---|---|---| +| `config_file` | the shipped config | Vehicle loadout, expanded at launch. | +| `name` | `x500` | Instance name: the Gazebo model name, the topic namespace (`//...`) and the TF prefix; letters, digits and underscores. See [Several vehicles](../../how-to/index.md#several-vehicles). | +| `x`, `y`, `z` | `0`, `0`, `0.25` | Spawn position (m); the default drops onto the landing gear. | +| `roll`, `pitch`, `yaw` | `0` | Spawn orientation (rad). | +| `world` | the ground world | Vehicle free world SDF to spawn into. | +| `gui` | `true` | Start the Gazebo GUI. | +| `use_composition` | `true` | Run the server, the bridge and the state publisher in one process. | + ## Choosing the world By default the vehicle is spawned into the vehicle free ground world @@ -32,10 +42,15 @@ ros2 launch x500_gazebo sim.launch.xml world:="$(ros2 pkg prefix --share x500_ga ## In RViz -To see the model in RViz: +To see the model in RViz, without Gazebo: ```bash ros2 launch x500_description display.launch.xml ``` +In a simulation the frames carry the instance name (`/base_link`) +and joint states arrive as `//joint_states`, so RViz next to +`sim.launch.xml` needs its fixed frame, its RobotModel TF prefix and its +joint states pointed at the instance. + ![](../../img/rviz.png) diff --git a/docs/vehicles/x500/sensors.md b/docs/vehicles/x500/sensors.md index 236a703..2e715e5 100644 --- a/docs/vehicles/x500/sensors.md +++ b/docs/vehicles/x500/sensors.md @@ -20,22 +20,23 @@ guide. | ROS Topic | Description | Message type | |---|---|---| -| `/x500/imu` | Body IMU | [sensor_msgs/msg/Imu](https://docs.ros.org/en/rolling/p/sensor_msgs/interfaces/msg/Imu.html) | -| `/x500/air_pressure` | Barometer | [sensor_msgs/msg/FluidPressure](https://docs.ros.org/en/rolling/p/sensor_msgs/interfaces/msg/FluidPressure.html) | -| `/x500/mag` | Magnetometer | [sensor_msgs/msg/MagneticField](https://docs.ros.org/en/rolling/p/sensor_msgs/interfaces/msg/MagneticField.html) | +| `//imu` | Body IMU | [sensor_msgs/msg/Imu](https://docs.ros.org/en/rolling/p/sensor_msgs/interfaces/msg/Imu.html) | +| `//air_pressure` | Barometer | [sensor_msgs/msg/FluidPressure](https://docs.ros.org/en/rolling/p/sensor_msgs/interfaces/msg/FluidPressure.html) | +| `//mag` | Magnetometer | [sensor_msgs/msg/MagneticField](https://docs.ros.org/en/rolling/p/sensor_msgs/interfaces/msg/MagneticField.html) | ### GPS | ROS Topic | Description | Message type | |---|---|---| -| `/x500/gps/fix` | Position fix | [sensor_msgs/msg/NavSatFix](https://docs.ros.org/en/rolling/p/sensor_msgs/interfaces/msg/NavSatFix.html) | - -These topic names are not fixed strings. Every part topic is built as -`///`, so the fix above is the `gps` -instance publishing `fix` under the `x500` namespace. Change the vehicle -and the topics change with it: - -- Empty the GPS slot and `/x500/gps/fix` goes away with the part. -- Rename the instance and the topic follows the new name. -- Set `topic`, `gz_topic` or `ros_topic` on the part in the config to - rename the base yourself. +| `//gps/fix` | Position fix | [sensor_msgs/msg/NavSatFix](https://docs.ros.org/en/rolling/p/sensor_msgs/interfaces/msg/NavSatFix.html) | + +`` is the instance name, `x500` for the default instance, or +whatever the quad was spawned as ([Several vehicles](../../how-to/index.md#several-vehicles)). +A part topic follows the part's own name (`gps` above), so a part renamed +or removed in the config moves or drops its topics +([Configuration](configuration.md)). + +Sensor messages carry the sensor's frame under the instance name +(`/base_link` for the flight sensors, `/gps_antenna` for the +GPS), which is the frame `robot_state_publisher` publishes for the +instance. diff --git a/x500_description/config/x500.yaml b/x500_description/config/x500.yaml index 02e7d5b..e78d498 100644 --- a/x500_description/config/x500.yaml +++ b/x500_description/config/x500.yaml @@ -25,6 +25,8 @@ base: {type: x500_airframe, name: base_link} # topic: base for BOTH the Gazebo and ROS sides # gz_topic: Gazebo side base (overrides `topic`) # ros_topic: ROS side base (overrides `topic`) +# A base goes under // unless it starts with a slash, +# which is then used as given (and shared by every instance). # bridge: dict of NATIVE ros_gz_bridge keys merged into that part's # generated bridge entries # diff --git a/x500_gazebo/CHANGELOG.rst b/x500_gazebo/CHANGELOG.rst index 651f960..e706a57 100644 --- a/x500_gazebo/CHANGELOG.rst +++ b/x500_gazebo/CHANGELOG.rst @@ -4,6 +4,8 @@ Changelog for package x500_gazebo Forthcoming ----------- +* The motor command bus is bridged to ROS as actuator_msgs/msg/Actuators on //command/motor_speed. +* One name per instance: configure_vehicle.py --name and the launch pose (x y z roll pitch yaw); joint states, topics, overrides and sensor frame ids under the instance name. * Motors and sensors placed by the assembly resolution; launch time loadouts via configure_vehicle.py; model://x500. * Initial import: generated composed model (four rotor actuators, flight sensors), aerial playground world, sim launch and generated ros_gz bridge. diff --git a/x500_gazebo/launch/sim.launch.xml b/x500_gazebo/launch/sim.launch.xml index 6b4eda0..67ccf97 100644 --- a/x500_gazebo/launch/sim.launch.xml +++ b/x500_gazebo/launch/sim.launch.xml @@ -9,7 +9,8 @@ config that does not match the parts (a slot that does not exist, a type that does not fit) fails the launch with the reason. The world is vehicle free (x500_ground.sdf by default) and the generated model is spawned into - it as "x500". With use_composition (default), gz_server, the bridge and + it under its instance name ("x500" unless name:= says otherwise), at the + pose x y z roll pitch yaw. With use_composition (default), gz_server, the bridge and robot_state_publisher run as composable nodes in ONE container process; the Gazebo GUI runs as a separate process. No control layer: the quad rests on its landing gear until something publishes motor commands. @@ -28,12 +29,18 @@ default="$(find-pkg-share x500_description)/config/x500.yaml" description="Vehicle loadout config, expanded at launch."/> + description="Instance name: the Gazebo model name, the topic namespace (/<name>/...) and the TF prefix; letters, digits and underscores. Give each copy of the vehicle in one world its own."/> + + + + + - + + value="$(command '$(find-pkg-prefix x500_gazebo)/lib/x500_gazebo/configure_vehicle.py --config $(var config_file) --name $(var name) --cache')"/> - + - + + + @@ -69,6 +83,8 @@ + + diff --git a/x500_gazebo/model.sdf.xacro b/x500_gazebo/model.sdf.xacro index 18394dc..4249143 100644 --- a/x500_gazebo/model.sdf.xacro +++ b/x500_gazebo/model.sdf.xacro @@ -26,8 +26,11 @@ + override the gz side base, under the namespace unless they start with + a slash; the bridge config is generated from the same yaml, so gz and + ROS sides stay in sync. The namespace is also the instance name: the + model name and, in a simulation, the prefix of every TF frame, so the + sensors' frame ids carry it. --> @@ -100,8 +103,9 @@ - ${frame} + carries it, under the instance name, which is the prefix + robot_state_publisher puts on every frame of this instance. --> + ${topic_ns}/${frame} ${topic_base}/fix 1 30 @@ -117,8 +121,9 @@ emit. --> + + value="${topic_override if topic_override.startswith('/') else topic_ns + '/' + topic_override}"/> @@ -131,13 +136,16 @@ - + + $(arg urdf_uri) + animate the props in RViz. Bridged to //joint_states. --> ${topic_ns}/joint_states @@ -158,7 +166,7 @@ - ${base_name} + ${topic_ns}/${base_name} ${topic_ns}/imu 1 250 @@ -177,7 +185,7 @@ - ${base_name} + ${topic_ns}/${base_name} ${topic_ns}/air_pressure 1 50 @@ -187,7 +195,7 @@ - ${base_name} + ${topic_ns}/${base_name} ${topic_ns}/mag 1 100 diff --git a/x500_gazebo/package.xml b/x500_gazebo/package.xml index 149ec91..06c9d59 100644 --- a/x500_gazebo/package.xml +++ b/x500_gazebo/package.xml @@ -28,6 +28,7 @@ ros_gz_bridge sensor_msgs + actuator_msgs launch launch_ros launch_xml diff --git a/x500_gazebo/scripts/configure_vehicle.py b/x500_gazebo/scripts/configure_vehicle.py index 453c3ba..53e1432 100755 --- a/x500_gazebo/scripts/configure_vehicle.py +++ b/x500_gazebo/scripts/configure_vehicle.py @@ -28,12 +28,20 @@ after the config's content, so the same loadout always lands in the same place and nothing piles up; its path is printed (nothing else), which is how the launch file calls it. + +--name gives the instance a name of its own: the Gazebo model name and the +topic namespace at once (topic_namespace in the config is overridden), so +several copies of the vehicle can share one world without their topics +colliding. The config that was actually used, with the override applied, is +written next to the artifacts as vehicle.yaml. Without --name the config's +topic_namespace (x500 by default) names the instance, as before. """ import argparse import hashlib import os import pathlib +import re import subprocess import sys @@ -44,7 +52,7 @@ MODEL_CONFIG = """\ - x500 + {name} 1.0 model.sdf Honu Robotics @@ -53,6 +61,34 @@ """ +# An instance name is a Gazebo model name, a topic prefix, a ROS namespace +# and a TF prefix at once, so it is held to the strictest of those. +NAME = re.compile(r'^[A-Za-z][A-Za-z0-9_]*$') + + +def derive(config, name=None): + """ + Return the effective config text. + + `config` with topic_namespace set to `name` when one is given. Everything + the generators read comes from this text, so the model name, the plugin + topics and the bridge agree. It is re-serialized, so the comments of the + given file do not carry over into vehicle.yaml; the values do. + """ + if name is not None and not NAME.match(name): + sys.exit(f'invalid instance name {name!r}: letters, digits and ' + 'underscores, starting with a letter') + cfg = yaml.safe_load(pathlib.Path(config).read_text()) or {} + if name: + cfg['topic_namespace'] = name + return yaml.safe_dump(cfg, sort_keys=False) + + +def instance_name(config_text): + """Return the name an instance goes by: its topic namespace.""" + return (yaml.safe_load(config_text) or {}).get('topic_namespace', 'x500') + + def run(cmd, what): out = subprocess.run(cmd, capture_output=True, text=True) if out.returncode != 0: @@ -60,14 +96,18 @@ def run(cmd, what): return out.stdout -def configure(config, out_dir): +def configure(config, out_dir, name=None): """Generate urdf, model.sdf, model.config and ros_gz_bridge.yaml into out_dir.""" desc = pathlib.Path(get_package_share_directory('x500_description')) gz = pathlib.Path(get_package_share_directory('x500_gazebo')) gz_lib = pathlib.Path(get_package_prefix('x500_gazebo')) / 'lib' / 'x500_gazebo' out_dir = pathlib.Path(out_dir) out_dir.mkdir(parents=True, exist_ok=True) - config = pathlib.Path(config).resolve() + # The config the generators read: the given one with the instance name + # applied, kept next to the artifacts it produced. + config_text = derive(config, name) + config = out_dir / 'vehicle.yaml' + config.write_text(config_text) vehicle_xacro = str(desc / 'urdf' / 'x500.urdf.xacro') urdf = out_dir / 'x500.urdf' @@ -82,35 +122,38 @@ def configure(config, out_dir): sdf.write_text(run(['xacro', str(gz / 'model.sdf.xacro'), f'config_file:={config}', f'urdf_uri:=file://{urdf}'], 'model generation')) - (out_dir / 'model.config').write_text(MODEL_CONFIG) + (out_dir / 'model.config').write_text( + MODEL_CONFIG.format(name=instance_name(config_text))) run([sys.executable, str(gz_lib / 'generate_bridge_config.py'), str(config), str(urdf), str(out_dir / 'ros_gz_bridge.yaml')], 'bridge config generation') return out_dir -def cache_dir(config): - """Per loadout directory under $ROS_HOME, keyed by the config's content.""" +def cache_dir(config, name=None): + """Per instance directory under $ROS_HOME, keyed by the effective config.""" ros_home = pathlib.Path(os.environ.get('ROS_HOME', pathlib.Path.home() / '.ros')) - digest = hashlib.sha256(pathlib.Path(config).read_bytes()).hexdigest()[:12] + digest = hashlib.sha256(derive(config, name).encode()).hexdigest()[:12] return ros_home / 'x500_gazebo' / digest def main(argv=None): ap = argparse.ArgumentParser(description=__doc__.split('\n\n')[0]) ap.add_argument('--config', required=True, help='vehicle config yaml') + ap.add_argument('--name', help='instance name: model name and topic namespace ' + '(overrides topic_namespace in the config)') group = ap.add_mutually_exclusive_group(required=True) group.add_argument('--out-dir', help='directory to write the artifacts into') group.add_argument('--cache', action='store_true', help='write into $ROS_HOME/x500_gazebo/, print its path') args = ap.parse_args(argv) - out_dir = cache_dir(args.config) if args.cache else args.out_dir - configure(args.config, out_dir) + out_dir = cache_dir(args.config, args.name) if args.cache else args.out_dir + configure(args.config, out_dir, args.name) if args.cache: sys.stdout.write(str(out_dir)) else: - print(f'wrote x500.urdf, model.sdf, model.config, ' + print(f'wrote vehicle.yaml, x500.urdf, model.sdf, model.config, ' f'ros_gz_bridge.yaml, robot_description.yaml to {out_dir}') diff --git a/x500_gazebo/scripts/generate_bridge_config.py b/x500_gazebo/scripts/generate_bridge_config.py index c9d2794..5ee6b38 100644 --- a/x500_gazebo/scripts/generate_bridge_config.py +++ b/x500_gazebo/scripts/generate_bridge_config.py @@ -16,15 +16,18 @@ Generate the ros_gz bridge config from the vehicle parts config. Run at build time (see CMakeLists.txt) and at launch (configure_vehicle.py): -emits /clock, /joint_states, the airframe flight sensors (IMU, air +emits /clock, //joint_states, the airframe flight sensors (IMU, air pressure, magnetometer: they belong to the airframe, not to a fitted part) and one entry per topic of each part in the assembly that has topics (the GPS part's fix). The parts come from the generated URDF's manifest (the resolved loadout, defaults included); topic bases follow //, overridable per part in the vehicle config with `topic` (both sides), `gz_topic` (Gazebo side) and `ros_topic` (ROS -side). The motor command bus (gz.msgs.Actuators) is deliberately NOT -bridged: that is the autopilot layer's interface. The config is also +side). An override stays under the namespace, so several instances of +one config never share a topic; one that starts with a slash is used as +given. The motor command bus (actuator_msgs/msg/Actuators to +gz.msgs.Actuators on //command/motor_speed) is bridged +too: it is the vehicle's actuator interface. The config is also checked against the manifest here (holybro_parts.assembly.check), so a loadout that names a slot or an instance that does not exist fails the build or the launch with the reason. @@ -59,6 +62,11 @@ def absolute(topic): return topic if topic.startswith('/') else '/' + topic +def under(ns, base): + """Return `base` under namespace `ns`, or as given when it starts with a slash.""" + return base if base.startswith('/') else f'{ns}/{base}' + + def overrides_for(cfg, name): """ Return the config entry that fitted instance `name`, or {}. @@ -89,12 +97,21 @@ def bridge_entries(cfg, instances): # Rotor joint states from the JointStatePublisher plugin, for # robot_state_publisher / RViz prop animation. entries.append({ - 'ros_topic_name': '/joint_states', + 'ros_topic_name': absolute(f'{ns}/joint_states'), 'gz_topic_name': absolute(f'{ns}/joint_states'), 'ros_type_name': 'sensor_msgs/msg/JointState', 'gz_type_name': 'gz.msgs.Model', 'direction': 'GZ_TO_ROS', }) + # The motor command bus: one angular velocity per rotor, indexed by the + # rotor number, the topic the MulticopterMotorModel plugins subscribe to. + entries.append({ + 'ros_topic_name': absolute(f'{ns}/command/motor_speed'), + 'gz_topic_name': absolute(f'{ns}/command/motor_speed'), + 'ros_type_name': 'actuator_msgs/msg/Actuators', + 'gz_type_name': 'gz.msgs.Actuators', + 'direction': 'ROS_TO_GZ', + }) for topic, ros_type, gz_type in FLIGHT_SENSORS: entries.append({ 'ros_topic_name': absolute(f'{ns}/{topic}'), @@ -106,9 +123,8 @@ def bridge_entries(cfg, instances): }) for ptype, name in instances: part = overrides_for(cfg, name) - default_base = f'{ns}/{name}' - gz_base = part.get('gz_topic', part.get('topic', default_base)) - ros_base = part.get('ros_topic', part.get('topic', default_base)) + gz_base = under(ns, part.get('gz_topic', part.get('topic', name))) + ros_base = under(ns, part.get('ros_topic', part.get('topic', name))) for suffix, ros_type, gz_type, direction in PART_TOPICS.get(ptype, []): entry = { 'ros_topic_name': absolute(f'{ros_base}/{suffix}'), diff --git a/x500_gazebo/test/test_bridge_generator.py b/x500_gazebo/test/test_bridge_generator.py index fd096ec..54a0f0a 100644 --- a/x500_gazebo/test/test_bridge_generator.py +++ b/x500_gazebo/test/test_bridge_generator.py @@ -28,7 +28,8 @@ # Airframe topics, always present (the flight sensors belong to the # airframe, not to a fitted part). -ALWAYS = {'/clock', '/joint_states', '/x500/imu', '/x500/air_pressure', '/x500/mag'} +ALWAYS = {'/clock', '/x500/joint_states', '/x500/command/motor_speed', + '/x500/imu', '/x500/air_pressure', '/x500/mag'} def entries_for(cfg, instances): @@ -46,6 +47,10 @@ def test_airframe_sensors_always_bridged(): assert imu['gz_type_name'] == 'gz.msgs.IMU' assert 'lazy' not in entries['/clock'] assert imu['lazy'] is True + bus = entries['/x500/command/motor_speed'] + assert bus['ros_type_name'] == 'actuator_msgs/msg/Actuators' + assert bus['gz_type_name'] == 'gz.msgs.Actuators' + assert bus['direction'] == 'ROS_TO_GZ' and 'lazy' not in bus def test_gps_part_bridges_its_fix(): @@ -66,24 +71,32 @@ def test_geometry_only_parts_produce_nothing(): def test_topic_and_namespace_overrides(): - """Gz_topic/ros_topic > topic > //, matched by instance.""" + """ + Gz_topic/ros_topic > topic > , matched by instance, under the namespace. + + An override stays under // so instances of one config never + share a topic; one starting with a slash is used as given. + """ cfg = {'topic_namespace': 'uav_a', 'parts': [ {'slot': 'gps', 'type': 'gps_mast', - 'gz_topic': 'uav_a/gps_raw', 'ros_topic': '/sensors/gps'}]} + 'gz_topic': 'gps_raw', 'ros_topic': '/sensors/gps'}]} entries = entries_for(cfg, [('gps_mast', 'gps')]) fix = entries['/sensors/gps/fix'] assert fix['gz_topic_name'] == '/uav_a/gps_raw/fix' assert '/uav_a/imu' in entries and '/x500/imu' not in entries + assert '/uav_a/joint_states' in entries + cfg = {'parts': [{'slot': 'gps', 'type': 'gps_mast', 'topic': 'rtk'}]} + assert '/x500/rtk/fix' in entries_for(cfg, [('gps_mast', 'gps')]) def test_extra_bridge_topics_verbatim(): - """Extra_bridge_topics entries are appended untouched (e.g. the motor bus).""" - extra = {'ros_topic_name': '/x500/motors', - 'gz_topic_name': '/x500/command/motor_speed', - 'ros_type_name': 'actuator_msgs/msg/Actuators', - 'gz_type_name': 'gz.msgs.Actuators', 'direction': 'ROS_TO_GZ'} + """Extra_bridge_topics entries are appended untouched.""" + extra = {'ros_topic_name': '/odom', + 'gz_topic_name': '/model/x500/odometry', + 'ros_type_name': 'nav_msgs/msg/Odometry', + 'gz_type_name': 'gz.msgs.Odometry', 'direction': 'GZ_TO_ROS'} cfg = {'extra_bridge_topics': [dict(extra)]} - assert entries_for(cfg, [])['/x500/motors'] == extra + assert entries_for(cfg, [])['/odom'] == extra def test_cli_rejects_a_config_that_matches_nothing(tmp_path): diff --git a/x500_gazebo/test/test_configure_vehicle.py b/x500_gazebo/test/test_configure_vehicle.py index 5af3f6c..477ce3a 100644 --- a/x500_gazebo/test/test_configure_vehicle.py +++ b/x500_gazebo/test/test_configure_vehicle.py @@ -106,3 +106,88 @@ def test_cache_mode_prints_only_the_directory(): again = subprocess.run([str(TOOL), '--config', str(DEFAULT_CONFIG), '--cache'], capture_output=True, text=True, timeout=180, env=env) assert again.stdout == out.stdout + + +def test_name_flows_to_every_artifact(tmp_path): + """--name renames the instance everywhere at once: model, topics, frames, bridge.""" + out = subprocess.run([str(TOOL), '--config', str(DEFAULT_CONFIG), '--name', 'uav_b', + '--out-dir', str(tmp_path / 'v')], + capture_output=True, text=True, timeout=180) + assert out.returncode == 0, out.stderr + out_dir = tmp_path / 'v' + # The config that was used, with the name applied, sits next to the artifacts. + used = yaml.safe_load((out_dir / 'vehicle.yaml').read_text()) + assert used['topic_namespace'] == 'uav_b' + # The model is named after the instance... + model = ET.parse(out_dir / 'model.sdf').getroot().find('model') + assert model.get('name') == 'uav_b' + assert ET.parse(out_dir / 'model.config').getroot().find('name').text == 'uav_b' + # ...and so is every topic and frame id its plugins and sensors use. + topics = [t.text for t in model.iter('topic')] + assert topics and all(t.startswith('uav_b/') for t in topics), topics + frames = [f.text for f in model.iter('frame_id')] + assert frames and all(f.startswith('uav_b/') for f in frames), frames + # The bridge agrees, so gz and ROS sides stay in sync. + bridge = yaml.safe_load((out_dir / 'ros_gz_bridge.yaml').read_text()) + gz_topics = {e['gz_topic_name'] for e in bridge} + assert '/uav_b/imu' in gz_topics and '/uav_b/joint_states' in gz_topics, gz_topics + assert not any(t.startswith('/x500/') for t in gz_topics), gz_topics + + +def test_name_gives_each_instance_its_own_cache_directory(): + """Two names from one config never share a directory; one name always does.""" + with tempfile.TemporaryDirectory() as ros_home: + env = dict(os.environ, ROS_HOME=ros_home) + + def cache(*extra): + out = subprocess.run([str(TOOL), '--config', str(DEFAULT_CONFIG), '--cache', *extra], + capture_output=True, text=True, timeout=180, env=env) + assert out.returncode == 0, out.stderr + return out.stdout + + default = cache() + a = cache('--name', 'uav_a') + b = cache('--name', 'uav_b') + assert len({default, a, b}) == 3 + assert cache('--name', 'uav_a') == a + # Without --name the config's own namespace names the instance, as before. + model = ET.parse(Path(default) / 'model.sdf').getroot().find('model') + assert model.get('name') == 'x500' + + +def test_two_instances_from_one_config_share_no_topic(tmp_path): + """ + Two names from one config, topic overrides included, share no topic. + + A relative override goes under the instance namespace on both sides, so + a message for one instance never reaches the other; only the fixed + /clock remains shared. + """ + cfg = yaml.safe_load(DEFAULT_CONFIG.read_text()) + cfg['parts'] = [{'slot': 'gps', 'type': 'gps_mast', 'topic': 'rtk'}] + config = tmp_path / 'custom.yaml' + config.write_text(yaml.safe_dump(cfg, sort_keys=False)) + topics = {} + for name in ('uav_a', 'uav_b'): + out = subprocess.run([str(TOOL), '--config', str(config), '--name', name, + '--out-dir', str(tmp_path / name)], + capture_output=True, text=True, timeout=180) + assert out.returncode == 0, out.stderr + model = ET.parse(tmp_path / name / 'model.sdf').getroot() + mine = {'/' + t.text.lstrip('/') for t in model.iter('topic')} + for entry in yaml.safe_load((tmp_path / name / 'ros_gz_bridge.yaml').read_text()): + mine |= {entry['gz_topic_name'], entry['ros_topic_name']} + topics[name] = mine - {'/clock'} + a, b = topics.values() + assert a and b and not (a & b), a & b + assert all(t.startswith('/uav_a/') for t in a), a + + +def test_rejects_a_name_that_is_not_a_ros_name(tmp_path): + """A name that cannot be a topic prefix or a namespace is refused up front.""" + for bad in ('uav-b', '2uavs', 'fleet/uav_b', ''): + out = subprocess.run([str(TOOL), '--config', str(DEFAULT_CONFIG), '--name', bad, + '--out-dir', str(tmp_path / 'v')], + capture_output=True, text=True, timeout=180) + assert out.returncode != 0, bad + assert 'invalid instance name' in out.stderr, bad diff --git a/x500_gazebo/test/test_model_generation.py b/x500_gazebo/test/test_model_generation.py index 889fedb..51b6734 100644 --- a/x500_gazebo/test/test_model_generation.py +++ b/x500_gazebo/test/test_model_generation.py @@ -129,6 +129,11 @@ def test_sensor_and_bridge_topics_agree(): root, _ = xacro(MODEL_XACRO, config) sdf_topics = {t.text if t.text.startswith('/') else '/' + t.text for t in root.iter('topic')} + # The motor bus has no : every motor plugin subscribes to its + # under its . + for motor in motor_plugins(root): + sdf_topics.add('/' + motor.find('robotNamespace').text + + '/' + motor.find('commandSubTopic').text) entries = bridge_gen.bridge_entries(yaml.safe_load(config), urdf_instances(config)) # /clock has no model-side ; joint_states does (the plugin's), @@ -164,8 +169,9 @@ def test_sensor_frame_ids_resolve_in_tf(): yields messages no lookup_transform can resolve. The deprecated gz_frame_id spelling also fails this test on purpose. """ - sdf_root, _ = xacro(MODEL_XACRO, default_config()) - urdf_root, _ = xacro(URDF_XACRO, default_config()) + config = default_config().replace('topic_namespace: x500', 'topic_namespace: uav_a') + sdf_root, _ = xacro(MODEL_XACRO, config) + urdf_root, _ = xacro(URDF_XACRO, config) urdf_links = {li.get('name') for li in urdf_root.findall('link')} sensors = list(sdf_root.iter('sensor')) assert sensors @@ -173,9 +179,19 @@ def test_sensor_frame_ids_resolve_in_tf(): frame = sensor.find('frame_id') assert frame is not None, ( f'sensor {sensor.get("name")} sets no ') - assert frame.text in urdf_links, ( + prefix, _, link = frame.text.partition('/') + assert prefix == 'uav_a' and link in urdf_links, ( f'sensor {sensor.get("name")} publishes frame_id {frame.text!r}, ' - f'which robot_state_publisher never puts in TF') + f'which robot_state_publisher never puts in TF for instance uav_a') + + +def test_model_name_follows_the_topic_namespace(): + """The model is named after the instance, so spawning it needs no rename.""" + root, _ = xacro(MODEL_XACRO, default_config()) + assert root.find('model').get('name') == 'x500' + root, _ = xacro(MODEL_XACRO, default_config().replace( + 'topic_namespace: x500', 'topic_namespace: uav_a')) + assert root.find('model').get('name') == 'uav_a' def test_installed_artifacts_match_shipped_config(): diff --git a/x500_gazebo/test/test_ros_launch.py b/x500_gazebo/test/test_ros_launch.py index d226a05..511b672 100644 --- a/x500_gazebo/test/test_ros_launch.py +++ b/x500_gazebo/test/test_ros_launch.py @@ -72,12 +72,26 @@ def test_robot_description_published(sim): def test_joint_states_flow(sim): - """Rotor joint states cross the bridge, so RViz can animate the props.""" - code, out, err = ros(sim, 'topic', 'echo', '/joint_states', '--once', + """Rotor joint states cross the bridge under the instance name, for RViz.""" + code, out, err = ros(sim, 'topic', 'echo', '/x500/joint_states', '--once', timeout=30) assert code == 0 and 'rotor_0_joint' in out, f'no joint states\n{err}' +def test_tf_frames_carry_the_instance_name(sim): + """robot_state_publisher prefixes every frame, matching the sensors' frame ids.""" + code, out, err = ros(sim, 'topic', 'echo', '/tf_static', '--once', + '--qos-durability', 'transient_local', + '--qos-reliability', 'reliable', timeout=30) + assert code == 0 and 'x500/gps_antenna' in out, f'no prefixed frames\n{err}' + + +def test_motor_bus_is_a_ros_topic(sim): + """The motor command bus is bridged to ROS as actuator messages.""" + code, out, err = ros(sim, 'topic', 'info', '/x500/command/motor_speed', timeout=30) + assert code == 0 and 'actuator_msgs/msg/Actuators' in out, f'motor bus not bridged\n{err}' + + def test_imu_flows_to_ros(sim): """IMU data crosses the bridge (non-render sensor: hard assertion).""" def imu_seen():