diff --git a/Tools/simulation/swarm_ros2/.gitignore b/Tools/simulation/swarm_ros2/.gitignore new file mode 100644 index 0000000000..087563af4b --- /dev/null +++ b/Tools/simulation/swarm_ros2/.gitignore @@ -0,0 +1,2 @@ +# Staging directory created by build_docker.sh (ephemeral) +_px4_msgs_defs/ diff --git a/Tools/simulation/swarm_ros2/Dockerfile b/Tools/simulation/swarm_ros2/Dockerfile new file mode 100644 index 0000000000..7e48a3d4ff --- /dev/null +++ b/Tools/simulation/swarm_ros2/Dockerfile @@ -0,0 +1,95 @@ +# ROS 2 Jazzy layer on top of PX4 dev environment +# For swarm simulation with px4-ros2-interface-lib +# +# Build (from PX4-Autopilot root): +# Tools/simulation/swarm_ros2/build_docker.sh +# +# Run: +# docker run -it --rm -v /path/to/PX4-Autopilot:/px4 px4-swarm-ros2 +# +FROM px4io/px4-dev:v1.17.0-rc1 +LABEL maintainer="Ramon Roche " + +ENV DEBIAN_FRONTEND=noninteractive +ENV ROS_DISTRO=jazzy + +# Add ROS 2 Jazzy repository (Ubuntu 24.04 = noble) +RUN apt-get update && apt-get install -y --no-install-recommends \ + curl \ + gnupg \ + lsb-release \ + software-properties-common \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \ + http://packages.ros.org/ros2/ubuntu noble main" \ + > /etc/apt/sources.list.d/ros2.list \ + && apt-get update \ + && apt-get install -y --no-install-recommends \ + ros-jazzy-ros-base \ + ros-jazzy-rmw-cyclonedds-cpp \ + python3-colcon-common-extensions \ + python3-rosdep \ + python3-numpy \ + && rm -rf /var/lib/apt/lists/* + +# Initialize rosdep +RUN rosdep init 2>/dev/null || true \ + && su user -c "rosdep update --rosdistro jazzy" 2>/dev/null || true + +# Set ROS 2 middleware to CycloneDDS (better multi-vehicle support) +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + +# Build Micro XRCE-DDS Agent from source +RUN git clone --depth 1 https://github.com/eProsima/Micro-XRCE-DDS-Agent.git /tmp/xrce-agent \ + && cd /tmp/xrce-agent \ + && mkdir build && cd build \ + && cmake .. -DCMAKE_BUILD_TYPE=Release \ + && make -j$(nproc) \ + && make install \ + && ldconfig \ + && rm -rf /tmp/xrce-agent + +# Build px4_msgs from PX4 source tree msg definitions (ensures type hash match). +# The build_docker.sh script copies msg/ and srv/ into _px4_msgs_defs/ before build. +ENV ROS2_WS=/opt/ros2_ws +COPY _px4_msgs_defs/ /tmp/px4_msg_defs/ +RUN mkdir -p ${ROS2_WS}/src \ + && cd ${ROS2_WS}/src \ + && git clone --depth 1 https://github.com/PX4/px4_msgs.git \ + && rm -rf px4_msgs/msg/*.msg px4_msgs/srv/*.srv \ + && cp /tmp/px4_msg_defs/msg/*.msg px4_msgs/msg/ \ + && cp /tmp/px4_msg_defs/srv/*.srv px4_msgs/srv/ \ + && rm -rf /tmp/px4_msg_defs \ + && cd ${ROS2_WS} \ + && . /opt/ros/jazzy/setup.sh \ + && colcon build --packages-select px4_msgs --cmake-args -DCMAKE_BUILD_TYPE=Release \ + && rm -rf log + +# Build px4-ros2-interface-lib (depends on px4_msgs built above) +RUN cd ${ROS2_WS}/src \ + && git clone --depth 1 --recursive https://github.com/Auterion/px4-ros2-interface-lib.git \ + && cd ${ROS2_WS} \ + && . /opt/ros/jazzy/setup.sh \ + && . install/setup.sh \ + && colcon build --packages-skip px4_msgs --cmake-args -DCMAKE_BUILD_TYPE=Release \ + && rm -rf log src + +# Copy swarm_ros2 package and build it +COPY . ${ROS2_WS}/src/swarm_ros2/ +RUN cd ${ROS2_WS} \ + && . /opt/ros/jazzy/setup.sh \ + && . install/setup.sh \ + && colcon build --packages-select swarm_ros2 \ + && rm -rf log + +# Source everything on shell entry +RUN echo "source /opt/ros/jazzy/setup.bash" >> /etc/bash.bashrc \ + && echo "source ${ROS2_WS}/install/setup.bash" >> /etc/bash.bashrc + +# XRCE-DDS UDP port +EXPOSE 8888/udp + +WORKDIR ${ROS2_WS} + +CMD ["/bin/bash"] diff --git a/Tools/simulation/swarm_ros2/build_docker.sh b/Tools/simulation/swarm_ros2/build_docker.sh new file mode 100755 index 0000000000..b4474ed173 --- /dev/null +++ b/Tools/simulation/swarm_ros2/build_docker.sh @@ -0,0 +1,35 @@ +#!/bin/bash +# Build the px4-swarm-ros2 Docker image with px4_msgs matching this PX4 commit. +# Run from anywhere; the script locates PX4 root automatically. +# +# Usage: ./build_docker.sh + +set -eo pipefail + +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +PX4_ROOT="$( cd "${SCRIPT_DIR}/../../.." && pwd )" + +echo "PX4 root: ${PX4_ROOT}" +echo "Staging px4_msgs definitions from this PX4 commit..." + +# Stage msg/srv files into the Docker build context +STAGING_DIR="${SCRIPT_DIR}/_px4_msgs_defs" +rm -rf "${STAGING_DIR}" +mkdir -p "${STAGING_DIR}/msg" "${STAGING_DIR}/srv" + +cp "${PX4_ROOT}"/msg/*.msg "${STAGING_DIR}/msg/" +cp "${PX4_ROOT}"/msg/versioned/*.msg "${STAGING_DIR}/msg/" +cp "${PX4_ROOT}"/srv/*.srv "${STAGING_DIR}/srv/" + +echo "Staged $(ls "${STAGING_DIR}/msg/" | wc -l | tr -d ' ') .msg files and $(ls "${STAGING_DIR}/srv/" | wc -l | tr -d ' ') .srv files" +echo "" +echo "Building Docker image..." + +docker build -t px4-swarm-ros2 "${SCRIPT_DIR}" + +# Clean up staged files +rm -rf "${STAGING_DIR}" + +echo "" +echo "Done. Run with:" +echo " docker run -it --rm -v ${PX4_ROOT}:/px4 px4-swarm-ros2 bash /px4/Tools/simulation/swarm_ros2/run_swarm.sh 4" diff --git a/Tools/simulation/swarm_ros2/launch/swarm_launch.py b/Tools/simulation/swarm_ros2/launch/swarm_launch.py new file mode 100644 index 0000000000..d097c9255f --- /dev/null +++ b/Tools/simulation/swarm_ros2/launch/swarm_launch.py @@ -0,0 +1,80 @@ +""" +Launch file for the PX4 swarm coordination stack. + +Starts one vision_provider node per drone and a single swarm_controller node. + +Usage: + ros2 launch swarm_ros2 swarm_launch.py num_drones:=16 speed_factor:=1.0 +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Declare launch arguments. + num_drones_arg = DeclareLaunchArgument( + 'num_drones', + default_value='16', + description='Number of drones in the swarm', + ) + speed_factor_arg = DeclareLaunchArgument( + 'speed_factor', + default_value='1.0', + description='Simulation speed factor', + ) + formation_config_arg = DeclareLaunchArgument( + 'formation_config', + default_value='', + description='Path to YAML formation configuration file', + ) + + num_drones = LaunchConfiguration('num_drones') + speed_factor = LaunchConfiguration('speed_factor') + + # We need a concrete integer to iterate, so we support a fixed max and + # conditionally launch. ROS 2 launch does not natively support dynamic + # loops over LaunchConfiguration integers, so we generate nodes for a + # reasonable upper bound and rely on the controller's num_drones param + # to limit actual usage. For simplicity, generate exactly 16 nodes + # (the default). For other counts, users can call this launch file + # programmatically or adjust the constant below. + MAX_DRONES = 16 + + vision_nodes = [] + for i in range(MAX_DRONES): + ns = f'px4_{i}' + vision_nodes.append( + Node( + package='swarm_ros2', + executable='vision_provider', + name=f'vision_provider_{i}', + parameters=[{ + 'namespace': ns, + 'instance_id': i, + }], + output='screen', + ) + ) + + # Single swarm controller. + controller_node = Node( + package='swarm_ros2', + executable='swarm_controller', + name='swarm_controller', + parameters=[{ + 'num_drones': num_drones, + 'speed_factor': speed_factor, + }], + output='screen', + ) + + return LaunchDescription([ + num_drones_arg, + speed_factor_arg, + formation_config_arg, + *vision_nodes, + controller_node, + ]) diff --git a/Tools/simulation/swarm_ros2/package.xml b/Tools/simulation/swarm_ros2/package.xml new file mode 100644 index 0000000000..680f241e0c --- /dev/null +++ b/Tools/simulation/swarm_ros2/package.xml @@ -0,0 +1,24 @@ + + + + swarm_ros2 + 0.1.0 + Swarm coordination for PX4 multi-vehicle simulation using px4-ros2-interface-lib concepts + Ramon Roche + BSD-3-Clause + + rclpy + px4_msgs + px4_ros2_cpp + std_msgs + geometry_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/Tools/simulation/swarm_ros2/resource/swarm_ros2 b/Tools/simulation/swarm_ros2/resource/swarm_ros2 new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/simulation/swarm_ros2/run_swarm.sh b/Tools/simulation/swarm_ros2/run_swarm.sh new file mode 100755 index 0000000000..1e351501e4 --- /dev/null +++ b/Tools/simulation/swarm_ros2/run_swarm.sh @@ -0,0 +1,99 @@ +#!/bin/bash +# Run the full swarm test inside Docker: PX4 SIH + XRCE-DDS + ROS 2 +# Usage: ./run_swarm.sh [NUM_DRONES] [SPEED_FACTOR] +# +# Must be run inside the px4-swarm-ros2 container with PX4 source mounted at /px4: +# docker run -it --rm -v /path/to/PX4-Autopilot:/px4 px4-swarm-ros2 /opt/ros2_ws/src/swarm_ros2/run_swarm.sh 4 + +set -eo pipefail + +NUM_DRONES="${1:-4}" +SPEED_FACTOR="${2:-1}" +PX4_SRC="${PX4_SRC:-/px4}" +BUILD_DIR="${PX4_SRC}/build/px4_sitl_sih" + +echo "=== PX4 SIH Swarm Test ===" +echo " Drones: $NUM_DRONES" +echo " Speed factor: $SPEED_FACTOR" +echo "" + +# Check PX4 build exists +if [ ! -f "$BUILD_DIR/bin/px4" ]; then + echo "PX4 binary not found at $BUILD_DIR/bin/px4" + echo "Building PX4 (this may take a few minutes)..." + cd "$PX4_SRC" + make px4_sitl_sih +fi + +# Source ROS 2 +source /opt/ros/jazzy/setup.bash +source /opt/ros2_ws/install/setup.bash + +# Cleanup handler +PIDS=() +cleanup() { + echo "" + echo "Shutting down..." + for pid in "${PIDS[@]}"; do + kill "$pid" 2>/dev/null || true + done + pkill -x px4 2>/dev/null || true + pkill -x MicroXRCEAgent 2>/dev/null || true + wait 2>/dev/null + echo "Done." +} +trap cleanup EXIT SIGINT SIGTERM + +# Kill any existing px4 +pkill -x px4 2>/dev/null || true +sleep 1 + +# Start XRCE-DDS agent +echo "Starting XRCE-DDS agent on port 8888..." +MicroXRCEAgent udp4 -p 8888 > /dev/null 2>&1 & +PIDS+=($!) +sleep 1 + +# Launch PX4 instances +export PX4_SIM_MODEL=sihsim_quadx_vision +export PX4_SIM_SPEED_FACTOR="$SPEED_FACTOR" + +# Clean stale instance data (old parameters.bson can override param set-default) +echo "Cleaning stale instance data..." +for n in $(seq 0 $((NUM_DRONES - 1))); do + rm -rf "$BUILD_DIR/instance_$n" +done + +echo "Launching $NUM_DRONES PX4 SIH instances..." +for n in $(seq 0 $((NUM_DRONES - 1))); do + working_dir="$BUILD_DIR/instance_$n" + mkdir -p "$working_dir" + + pushd "$working_dir" &>/dev/null + # Force consistent DDS namespace for all instances (including 0) + # so ROS 2 nodes can use px4_0, px4_1, ... uniformly + PX4_UXRCE_DDS_NS="px4_$n" "$BUILD_DIR/bin/px4" -i "$n" -d "$BUILD_DIR/etc" >out.log 2>err.log & + PIDS+=($!) + popd &>/dev/null + echo " Instance $n started (PID ${PIDS[-1]})" +done + +# Wait for PX4 instances to initialize +echo "" +echo "Waiting 5s for PX4 instances to initialize..." +sleep 5 + +# Launch ROS 2 swarm (blocks until mission completes) +echo "" +echo "Launching ROS 2 swarm controller..." +echo "==========================================" +ros2 launch swarm_ros2 swarm_launch.py num_drones:="$NUM_DRONES" + +echo "" +echo "=== Mission complete ===" + +# Collect ULogs +echo "Collecting ULog files..." +python3 "$PX4_SRC/Tools/simulation/collect_swarm_ulogs.py" \ + --build-dir "$BUILD_DIR" \ + --output-dir "$PX4_SRC/swarm_ulogs_$(date +%Y%m%d_%H%M%S)" diff --git a/Tools/simulation/swarm_ros2/setup.cfg b/Tools/simulation/swarm_ros2/setup.cfg new file mode 100644 index 0000000000..b2774d14cc --- /dev/null +++ b/Tools/simulation/swarm_ros2/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/swarm_ros2 + +[install] +install_scripts=$base/lib/swarm_ros2 diff --git a/Tools/simulation/swarm_ros2/setup.py b/Tools/simulation/swarm_ros2/setup.py new file mode 100644 index 0000000000..6e0bbe9a82 --- /dev/null +++ b/Tools/simulation/swarm_ros2/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'swarm_ros2' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Ramon Roche', + maintainer_email='mrpollo@gmail.com', + description='Swarm coordination for PX4 multi-vehicle simulation', + license='BSD-3-Clause', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'vision_provider = swarm_ros2.vision_provider:main', + 'swarm_controller = swarm_ros2.swarm_controller:main', + ], + }, +) diff --git a/Tools/simulation/swarm_ros2/swarm_ros2/__init__.py b/Tools/simulation/swarm_ros2/swarm_ros2/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/simulation/swarm_ros2/swarm_ros2/swarm_controller.py b/Tools/simulation/swarm_ros2/swarm_ros2/swarm_controller.py new file mode 100644 index 0000000000..b945150465 --- /dev/null +++ b/Tools/simulation/swarm_ros2/swarm_ros2/swarm_controller.py @@ -0,0 +1,599 @@ +""" +Swarm controller node for PX4 multi-vehicle coordination. + +Manages all drones through a phased state machine: + 1. Wait for EV lock + 2. Arm + takeoff to formation + 3. Hold grid formation + 4. Transition to circle via cosine interpolation + 5. Hold circle and rotate + 6. Kamikaze dive to target + +Uses PX4 offboard control via px4_msgs topics. +""" + +import math +import os +from enum import IntEnum, auto + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy + +from px4_msgs.msg import ( + OffboardControlMode, + TrajectorySetpoint, + VehicleCommand, + VehicleLocalPosition, + VehicleStatus, +) + + +PX4_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + depth=1, +) + +# Volatile QoS for subscriptions to avoid stale data from before +# namespace discovery is complete. +PX4_SUB_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + history=HistoryPolicy.KEEP_LAST, + depth=1, +) + + +class Phase(IntEnum): + """State machine phases.""" + WAIT_EV_LOCK = auto() + ARM_TAKEOFF = auto() + HOLD_GRID = auto() + TRANSITION_CIRCLE = auto() + HOLD_CIRCLE = auto() + KAMIKAZE = auto() + DONE = auto() + + +class DroneState: + """Per-drone bookkeeping.""" + + def __init__(self, drone_id: int, namespace: str): + self.drone_id = drone_id + self.namespace = namespace + + # Telemetry + self.xy_valid = False + self.z_valid = False + self.position = np.zeros(3) # NED [x, y, z] + self.has_initial_pos = False + self._init_samples: list = [] # Buffer for initial position consensus + self.armed = False + self.nav_state = 0 + + # Formation target (grid) + self.grid_pos = np.zeros(3) + + # Circle target + self.circle_angle = 0.0 + + # Mission status + self.impacted = False + + +class SwarmController(Node): + """Coordinates N drones through the swarm mission phases.""" + + # Phase durations in seconds + EV_LOCK_TIMEOUT = 15.0 + TAKEOFF_DURATION = 20.0 + HOLD_GRID_DURATION = 10.0 + TRANSITION_DURATION = 15.0 + HOLD_CIRCLE_DURATION = 10.0 + KAMIKAZE_TIMEOUT = 120.0 + + # Circle parameters + CIRCLE_RADIUS = 4.0 # metres + CIRCLE_SPEED = 3.0 # m/s tangential + + # Kamikaze parameters + KAMIKAZE_SPEED = 12.0 # m/s (MPC_XY_VEL_MAX) + IMPACT_DIST = 2.0 # metres + IMPACT_Z_THRESH = -1.0 # NED (z > -1 means within 1m of ground) + + # PX4 command constants + VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 + VEHICLE_CMD_DO_SET_MODE = 176 + + CONTROL_HZ = 20.0 + + def __init__(self): + super().__init__('swarm_controller') + + # Parameters + self.declare_parameter('num_drones', 16) + self.declare_parameter('speed_factor', 1.0) + self.declare_parameter('target_x', 200.0) + self.declare_parameter('target_y', 0.0) + self.declare_parameter('target_z', 0.0) + self.declare_parameter('takeoff_altitude', -5.0) # NED, negative = up + + self._num_drones = self.get_parameter('num_drones').value + self._speed_factor = self.get_parameter('speed_factor').value + + # Kamikaze target (NED) + self._target = np.array([ + float(os.environ.get('PX4_KAMIKAZE_X', + self.get_parameter('target_x').value)), + float(os.environ.get('PX4_KAMIKAZE_Y', + self.get_parameter('target_y').value)), + float(os.environ.get('PX4_KAMIKAZE_Z', + self.get_parameter('target_z').value)), + ]) + + takeoff_alt = self.get_parameter('takeoff_altitude').value + + self.get_logger().info( + f'Swarm controller: {self._num_drones} drones, ' + f'speed_factor={self._speed_factor}, ' + f'target={self._target.tolist()}' + ) + + # Build grid formation (4x4 or NxM, 2m spacing, centered at origin). + cols = int(math.ceil(math.sqrt(self._num_drones))) + rows = int(math.ceil(self._num_drones / cols)) + grid_center_x = (cols - 1) / 2.0 + grid_center_y = (rows - 1) / 2.0 + + # ------------------------------------------------------------------ # + # Per-drone state and ROS interfaces + # ------------------------------------------------------------------ # + self._drones: list[DroneState] = [] + self._pub_offboard: list[rclpy.publisher.Publisher] = [] + self._pub_traj: list[rclpy.publisher.Publisher] = [] + self._pub_cmd: list[rclpy.publisher.Publisher] = [] + + for i in range(self._num_drones): + ns = f'px4_{i}' + drone = DroneState(i, ns) + + # Grid position: 2m spacing, centered, at takeoff altitude. + col = i % cols + row = i // cols + drone.grid_pos = np.array([ + (col - grid_center_x) * 2.0, + (row - grid_center_y) * 2.0, + float(takeoff_alt), + ]) + + # Circle angle for this drone. + drone.circle_angle = 2.0 * math.pi * i / self._num_drones + + self._drones.append(drone) + + # Publishers + self._pub_offboard.append( + self.create_publisher( + OffboardControlMode, + f'/{ns}/fmu/in/offboard_control_mode', + PX4_QOS, + ) + ) + self._pub_traj.append( + self.create_publisher( + TrajectorySetpoint, + f'/{ns}/fmu/in/trajectory_setpoint', + PX4_QOS, + ) + ) + self._pub_cmd.append( + self.create_publisher( + VehicleCommand, + f'/{ns}/fmu/in/vehicle_command', + PX4_QOS, + ) + ) + + # Subscribers + # PX4 XRCE-DDS uses versioned topic names: _v + # VehicleLocalPosition: MESSAGE_VERSION=1 -> _v1 + # VehicleStatus: MESSAGE_VERSION=2 -> _v2 + self.create_subscription( + VehicleLocalPosition, + f'/{ns}/fmu/out/vehicle_local_position_v1', + lambda msg, idx=i: self._local_pos_cb(idx, msg), + PX4_SUB_QOS, + ) + self.create_subscription( + VehicleStatus, + f'/{ns}/fmu/out/vehicle_status_v2', + lambda msg, idx=i: self._vehicle_status_cb(idx, msg), + PX4_SUB_QOS, + ) + + # ------------------------------------------------------------------ # + # State machine + # ------------------------------------------------------------------ # + self._phase = Phase.WAIT_EV_LOCK + self._phase_start_time = self.get_clock().now() + self._mission_start_time = self.get_clock().now() + self._circle_center = np.array([0.0, 0.0, float(takeoff_alt)]) + self._last_ev_print = 0 + + # Main control timer + period_s = 1.0 / self.CONTROL_HZ + self._timer = self.create_timer(period_s, self._control_loop) + + # ===================================================================== # + # Subscriber callbacks + # ===================================================================== # + def _local_pos_cb(self, idx: int, msg: VehicleLocalPosition): + d = self._drones[idx] + d.xy_valid = msg.xy_valid + d.z_valid = msg.z_valid + + new_pos = np.array([msg.x, msg.y, msg.z]) + + if not d.has_initial_pos: + if not (d.xy_valid and d.z_valid): + return + # All drones start near origin. Reject samples > 20m from origin + # during initialization (DDS cross-talk from other instances). + if np.linalg.norm(new_pos) > 20.0: + return + # Require 5 consistent samples within 5m to establish initial pos. + d._init_samples.append(new_pos) + if len(d._init_samples) >= 5: + ref = d._init_samples[-1] + consistent = all( + np.linalg.norm(s - ref) < 5.0 + for s in d._init_samples[-5:] + ) + if consistent: + d.position = new_pos + d.has_initial_pos = True + else: + d._init_samples = d._init_samples[-1:] + return + + # Guard against DDS cross-talk: reject position jumps > 50m/tick. + # At 50Hz and 12m/s max, real movement is < 0.3m per tick. + jump = np.linalg.norm(new_pos - d.position) + if jump > 50.0: + return # Discard cross-talk sample + + d.position = new_pos + + def _vehicle_status_cb(self, idx: int, msg: VehicleStatus): + d = self._drones[idx] + d.armed = (msg.arming_state == VehicleStatus.ARMING_STATE_ARMED) + d.nav_state = msg.nav_state + + # ===================================================================== # + # Publishing helpers + # ===================================================================== # + def _publish_offboard_mode(self, idx: int, *, position: bool = False, + velocity: bool = False): + """Send OffboardControlMode for drone idx.""" + msg = OffboardControlMode() + msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) + msg.position = position + msg.velocity = velocity + msg.acceleration = False + msg.attitude = False + msg.body_rate = False + self._pub_offboard[idx].publish(msg) + + def _publish_trajectory(self, idx: int, *, + position: list[float] | None = None, + velocity: list[float] | None = None, + yaw: float = float('nan'), + yawspeed: float = float('nan')): + """Send TrajectorySetpoint for drone idx.""" + msg = TrajectorySetpoint() + msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) + + nan3 = [float('nan')] * 3 + msg.position = list(position) if position is not None else nan3 + msg.velocity = list(velocity) if velocity is not None else nan3 + msg.yaw = yaw + msg.yawspeed = yawspeed + self._pub_traj[idx].publish(msg) + + def _publish_vehicle_command(self, idx: int, command: int, + param1: float = 0.0, param2: float = 0.0, + param3: float = 0.0): + """Send VehicleCommand for drone idx.""" + msg = VehicleCommand() + msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) + msg.command = command + msg.param1 = param1 + msg.param2 = param2 + msg.param3 = param3 + msg.target_system = idx + 1 # MAV_SYS_ID + msg.target_component = 1 + msg.source_system = 255 # GCS + msg.source_component = 0 + msg.from_external = True + self._pub_cmd[idx].publish(msg) + + def _arm(self, idx: int): + self._publish_vehicle_command( + idx, self.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=1.0) + + def _disarm_force(self, idx: int): + self._publish_vehicle_command( + idx, self.VEHICLE_CMD_COMPONENT_ARM_DISARM, + param1=0.0, param2=21196.0) + + def _set_offboard_mode(self, idx: int): + self._publish_vehicle_command( + idx, self.VEHICLE_CMD_DO_SET_MODE, + param1=1.0, param2=6.0, param3=0.0) + + # ===================================================================== # + # Phase elapsed time helper + # ===================================================================== # + def _phase_elapsed(self) -> float: + """Seconds since current phase started.""" + return (self.get_clock().now() - self._phase_start_time).nanoseconds / 1e9 + + def _mission_elapsed(self) -> float: + """Seconds since mission started.""" + return (self.get_clock().now() - self._mission_start_time).nanoseconds / 1e9 + + def _advance_phase(self, new_phase: Phase): + self.get_logger().info(f'Phase transition: {self._phase.name} -> {new_phase.name}') + self._phase = new_phase + self._phase_start_time = self.get_clock().now() + + # ===================================================================== # + # Main control loop (20 Hz) + # ===================================================================== # + def _control_loop(self): + if self._phase == Phase.DONE: + return + + if self._phase == Phase.WAIT_EV_LOCK: + self._phase_wait_ev_lock() + elif self._phase == Phase.ARM_TAKEOFF: + self._phase_arm_takeoff() + elif self._phase == Phase.HOLD_GRID: + self._phase_hold_grid() + elif self._phase == Phase.TRANSITION_CIRCLE: + self._phase_transition_circle() + elif self._phase == Phase.HOLD_CIRCLE: + self._phase_hold_circle() + elif self._phase == Phase.KAMIKAZE: + self._phase_kamikaze() + + # ------------------------------------------------------------------ # + # Phase 1: Wait for EV lock + # ------------------------------------------------------------------ # + def _phase_wait_ev_lock(self): + locked = sum(1 for d in self._drones if d.xy_valid and d.z_valid) + elapsed = self._phase_elapsed() + + # Print status every second. + elapsed_int = int(elapsed) + if elapsed_int > self._last_ev_print: + self._last_ev_print = elapsed_int + self.get_logger().info( + f'{locked}/{self._num_drones} drones have EV lock ' + f'({elapsed:.0f}s elapsed)') + + if locked == self._num_drones or elapsed > self.EV_LOCK_TIMEOUT: + if locked < self._num_drones: + self.get_logger().warn( + f'EV lock timeout: only {locked}/{self._num_drones} locked') + self._advance_phase(Phase.ARM_TAKEOFF) + + # ------------------------------------------------------------------ # + # Phase 2: Arm + takeoff + # ------------------------------------------------------------------ # + def _phase_arm_takeoff(self): + elapsed = self._phase_elapsed() + + for i in range(self._num_drones): + d = self._drones[i] + + # Keep sending offboard + setpoint before and after arming. + self._publish_offboard_mode(i, position=True) + self._publish_trajectory( + i, + position=d.grid_pos.tolist(), + yaw=0.0, + ) + + # Arm and set offboard mode repeatedly for the first few seconds. + if elapsed < 5.0: + self._set_offboard_mode(i) + self._arm(i) + + # Log status every 5 seconds. + elapsed_int = int(elapsed) + if elapsed_int > 0 and elapsed_int % 5 == 0 and elapsed - elapsed_int < 0.1: + armed = sum(1 for d in self._drones if d.armed) + self.get_logger().info( + f'ARM_TAKEOFF {elapsed:.0f}s: {armed}/{self._num_drones} armed, ' + f'positions: ' + ', '.join( + f'D{i}=[{d.position[2]:.1f}m]' + for i, d in enumerate(self._drones) + ) + ) + + if elapsed > self.TAKEOFF_DURATION: + self._advance_phase(Phase.HOLD_GRID) + + # ------------------------------------------------------------------ # + # Phase 3: Hold grid + # ------------------------------------------------------------------ # + def _phase_hold_grid(self): + for i in range(self._num_drones): + d = self._drones[i] + self._publish_offboard_mode(i, position=True) + self._publish_trajectory( + i, + position=d.grid_pos.tolist(), + yaw=0.0, + ) + + if self._phase_elapsed() > self.HOLD_GRID_DURATION: + # Compute circle center as the mean of grid positions. + mean_pos = np.mean([d.grid_pos for d in self._drones], axis=0) + self._circle_center = mean_pos.copy() + self._advance_phase(Phase.TRANSITION_CIRCLE) + + # ------------------------------------------------------------------ # + # Phase 4: Transition to circle (cosine interpolation) + # ------------------------------------------------------------------ # + def _phase_transition_circle(self): + elapsed = self._phase_elapsed() + alpha = 0.5 - 0.5 * math.cos(math.pi * min(elapsed / self.TRANSITION_DURATION, 1.0)) + + for i in range(self._num_drones): + d = self._drones[i] + + # Target circle position. + cx = self._circle_center[0] + self.CIRCLE_RADIUS * math.cos(d.circle_angle) + cy = self._circle_center[1] + self.CIRCLE_RADIUS * math.sin(d.circle_angle) + cz = self._circle_center[2] + circle_pos = np.array([cx, cy, cz]) + + # Interpolate between grid and circle. + target = (1.0 - alpha) * d.grid_pos + alpha * circle_pos + + # Yaw facing center. + yaw = math.atan2( + self._circle_center[1] - target[1], + self._circle_center[0] - target[0], + ) + + self._publish_offboard_mode(i, position=True) + self._publish_trajectory(i, position=target.tolist(), yaw=yaw) + + if elapsed > self.TRANSITION_DURATION: + self._advance_phase(Phase.HOLD_CIRCLE) + + # ------------------------------------------------------------------ # + # Phase 5: Hold circle (rotate CW) + # ------------------------------------------------------------------ # + def _phase_hold_circle(self): + elapsed = self._phase_elapsed() + omega = self.CIRCLE_SPEED / self.CIRCLE_RADIUS # rad/s + + for i in range(self._num_drones): + d = self._drones[i] + # CW rotation: subtract omega*t from angle. + angle = d.circle_angle - omega * elapsed + + px = self._circle_center[0] + self.CIRCLE_RADIUS * math.cos(angle) + py = self._circle_center[1] + self.CIRCLE_RADIUS * math.sin(angle) + pz = self._circle_center[2] + + # Yaw facing center. + yaw = math.atan2( + self._circle_center[1] - py, + self._circle_center[0] - px, + ) + + self._publish_offboard_mode(i, position=True) + self._publish_trajectory( + i, + position=[px, py, pz], + yaw=yaw, + ) + + if elapsed > self.HOLD_CIRCLE_DURATION: + self._advance_phase(Phase.KAMIKAZE) + + # ------------------------------------------------------------------ # + # Phase 6: Kamikaze dive + # ------------------------------------------------------------------ # + def _phase_kamikaze(self): + all_done = True + elapsed = self._phase_elapsed() + + for i in range(self._num_drones): + d = self._drones[i] + + if d.impacted: + continue + all_done = False + + # Direction to target. + to_target = self._target - d.position + dist = np.linalg.norm(to_target) + + # Impact detection. + if dist < self.IMPACT_DIST and d.position[2] > self.IMPACT_Z_THRESH: + self.get_logger().info( + f'[Drone {i}] Impact! pos=[{d.position[0]:.1f}, ' + f'{d.position[1]:.1f}, {d.position[2]:.1f}]') + d.impacted = True + self._disarm_force(i) + continue + + # Velocity toward target. + if dist > 0.01: + direction = to_target / dist + else: + direction = np.array([1.0, 0.0, 0.0]) + vel = (direction * self.KAMIKAZE_SPEED).tolist() + + # Yaw toward target. + yaw = math.atan2(to_target[1], to_target[0]) + + self._publish_offboard_mode(i, velocity=True) + self._publish_trajectory(i, velocity=vel, yaw=yaw) + + # Log kamikaze progress every 10 seconds. + elapsed_int = int(elapsed) + if elapsed_int > 0 and elapsed_int % 10 == 0 and elapsed - elapsed_int < 0.1: + for i, d in enumerate(self._drones): + if not d.impacted: + dist = np.linalg.norm(self._target - d.position) + self.get_logger().info( + f'KAMIKAZE {elapsed:.0f}s: D{i} pos=[{d.position[0]:.1f}, ' + f'{d.position[1]:.1f}, {d.position[2]:.1f}] ' + f'dist={dist:.1f}m armed={d.armed}') + + if all_done or elapsed > self.KAMIKAZE_TIMEOUT: + self._print_summary() + self._advance_phase(Phase.DONE) + # Shutdown after a short delay to let final messages flush. + self.create_timer(1.0, lambda: self._shutdown()) + + def _print_summary(self): + impacted = sum(1 for d in self._drones if d.impacted) + total_time = self._mission_elapsed() + self.get_logger().info('=' * 50) + self.get_logger().info('SWARM MISSION SUMMARY') + self.get_logger().info(f' Drones: {self._num_drones}') + self.get_logger().info(f' Impacts: {impacted}/{self._num_drones}') + self.get_logger().info(f' Total time: {total_time:.1f}s') + self.get_logger().info('=' * 50) + + def _shutdown(self): + self.get_logger().info('Swarm controller shutting down.') + self._timer.cancel() + raise SystemExit(0) + + +def main(args=None): + rclpy.init(args=args) + node = SwarmController() + try: + rclpy.spin(node) + except (KeyboardInterrupt, SystemExit): + node.get_logger().info('Swarm controller exiting.') + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/Tools/simulation/swarm_ros2/swarm_ros2/vision_provider.py b/Tools/simulation/swarm_ros2/swarm_ros2/vision_provider.py new file mode 100644 index 0000000000..dba31e459d --- /dev/null +++ b/Tools/simulation/swarm_ros2/swarm_ros2/vision_provider.py @@ -0,0 +1,156 @@ +""" +Vision provider node for PX4 EKF2 external vision fusion. + +Subscribes to ground truth local position and publishes noisy visual odometry +estimates, simulating an external vision system (e.g., motion capture, VIO). + +One instance runs per drone. The node decimates to 30 Hz and adds configurable +Gaussian noise to position and velocity. +""" + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy + +from px4_msgs.msg import VehicleLocalPosition as VehicleLocalPositionGroundtruth +from px4_msgs.msg import VehicleOdometry + + +# QoS profile matching PX4 uORB-to-DDS bridge defaults. +PX4_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + depth=1, +) + + +class VisionProvider(Node): + """Publishes noisy VehicleOdometry derived from ground truth.""" + + # Noise standard deviations + POS_NOISE_STD = 0.5 # metres + ANG_VEL_NOISE_STD = 0.1 # rad/s (unused but kept for completeness) + + # Output rate target + OUTPUT_HZ = 30.0 + OUTPUT_PERIOD_US = int(1e6 / OUTPUT_HZ) + + def __init__(self): + # Declare parameters before calling super().__init__ so they are + # available immediately. + super().__init__('vision_provider') + + self.declare_parameter('namespace', 'px4_0') + self.declare_parameter('instance_id', 0) + + self._namespace = self.get_parameter('namespace').value + self._instance_id = self.get_parameter('instance_id').value + + self.get_logger().info( + f'Starting vision_provider_{self._instance_id} ' + f'for namespace /{self._namespace}' + ) + self.get_logger().warn( + 'Note: vehicle_local_position_groundtruth is NOT published ' + 'over DDS by PX4. This node is a no-op when using ' + 'sensor_agp_sim (SENS_EN_AGPSIM=1) for position. ' + 'It will only produce output if EV fusion is enabled and ' + 'ground truth is published via a custom DDS bridge.' + ) + + self._last_pub_time_us: int = 0 + self._msg_count: int = 0 + self._rng = np.random.default_rng() + + # -- Subscriber: ground truth local position -- + # Note: PX4 SIH does not publish this topic over XRCE-DDS. + # With sensor_agp_sim, EKF2 gets position internally. + gt_topic = f'/{self._namespace}/fmu/out/vehicle_local_position_groundtruth' + self._sub_gt = self.create_subscription( + VehicleLocalPositionGroundtruth, + gt_topic, + self._gt_callback, + PX4_QOS, + ) + + # -- Publisher: visual odometry for EKF2 -- + vo_topic = f'/{self._namespace}/fmu/in/vehicle_visual_odometry' + self._pub_vo = self.create_publisher(VehicleOdometry, vo_topic, PX4_QOS) + + # --------------------------------------------------------------------- # + # Callback + # --------------------------------------------------------------------- # + def _gt_callback(self, msg: VehicleLocalPositionGroundtruth): + """Decimate ground truth and publish noisy visual odometry.""" + now_us = int(self.get_clock().now().nanoseconds / 1000) + + # Rate-limit to OUTPUT_HZ. + if (now_us - self._last_pub_time_us) < self.OUTPUT_PERIOD_US: + return + self._last_pub_time_us = now_us + + # Build VehicleOdometry message. + vo = VehicleOdometry() + vo.timestamp = now_us + vo.timestamp_sample = msg.timestamp + + # Frames: NED + vo.pose_frame = VehicleOdometry.POSE_FRAME_NED + vo.velocity_frame = VehicleOdometry.VELOCITY_FRAME_NED + + # Position with Gaussian noise. + noise_pos = self._rng.normal(0.0, self.POS_NOISE_STD, size=3).astype(np.float32) + vo.position = [ + msg.x + float(noise_pos[0]), + msg.y + float(noise_pos[1]), + msg.z + float(noise_pos[2]), + ] + + # Quaternion (pass through, no noise on orientation). + vo.q = [msg.heading, 0.0, 0.0, 0.0] + # If ground truth provides a full quaternion, prefer that. The + # VehicleLocalPosition message only carries heading; construct a + # yaw-only quaternion. + half_yaw = msg.heading / 2.0 + vo.q = [ + float(np.cos(half_yaw)), + 0.0, + 0.0, + float(np.sin(half_yaw)), + ] + + # Velocity with small noise. + noise_vel = self._rng.normal(0.0, 0.1, size=3).astype(np.float32) + vo.velocity = [ + msg.vx + float(noise_vel[0]), + msg.vy + float(noise_vel[1]), + msg.vz + float(noise_vel[2]), + ] + + # Angular velocity (not used, set to zero). + vo.angular_velocity = [0.0, 0.0, 0.0] + + # Variances (squared standard deviations). + vo.position_variance = [0.25, 0.25, 0.25] # 0.5^2 + vo.orientation_variance = [0.01, 0.01, 0.01] + vo.velocity_variance = [0.1, 0.1, 0.1] + + self._pub_vo.publish(vo) + + +def main(args=None): + rclpy.init(args=args) + node = VisionProvider() + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info('Shutting down vision provider.') + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main()