mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 08:53:34 +08:00
feat(simulation): add ROS 2 swarm controller package
ROS 2 package for coordinating PX4 SIH multi-drone swarm missions via XRCE-DDS. Includes: - swarm_controller.py: orchestrates N drones through 6 phases (EV lock, arm/takeoff, grid hold, circle transition, circle hold, kamikaze dive) with DDS cross-talk filtering for multi-instance XRCE-DDS setups - vision_provider.py: publishes noisy VehicleOdometry from ground truth for EKF2 external vision fusion (placeholder for non-GPS) - swarm_launch.py: ROS 2 launch file for N vision providers + controller - run_swarm.sh: Docker entrypoint that launches PX4 instances, XRCE-DDS agent, and ROS 2 nodes - Dockerfile + build_docker.sh: containerized build with ROS 2 Jazzy, px4_msgs from PX4 source, and the swarm package Validated with 4 drones: all phases complete, 4/4 impacts, ULogs generated per instance (~16 MB each). Signed-off-by: Ramon Roche <mrpollo@gmail.com>
This commit is contained in:
Ramon Roche
@@ -0,0 +1,2 @@
|
||||
# Staging directory created by build_docker.sh (ephemeral)
|
||||
_px4_msgs_defs/
|
||||
@@ -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 <mrpollo@gmail.com>"
|
||||
|
||||
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"]
|
||||
Executable
+35
@@ -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"
|
||||
@@ -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,
|
||||
])
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>swarm_ros2</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Swarm coordination for PX4 multi-vehicle simulation using px4-ros2-interface-lib concepts</description>
|
||||
<maintainer email="mrpollo@gmail.com">Ramon Roche</maintainer>
|
||||
<license>BSD-3-Clause</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>px4_msgs</depend>
|
||||
<depend>px4_ros2_cpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Executable
+99
@@ -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)"
|
||||
@@ -0,0 +1,5 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/swarm_ros2
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/swarm_ros2
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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<MESSAGE_VERSION>
|
||||
# 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()
|
||||
@@ -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()
|
||||
Reference in New Issue
Block a user