Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

SITL: Multi-vehicle sim with AirSim + ROS #12930

Merged
merged 2 commits into from Dec 5, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
33 changes: 33 additions & 0 deletions libraries/SITL/examples/Airsim/multi_uav_ros_sitl.launch
@@ -0,0 +1,33 @@
<?xml version="1.0"?>
<launch>
<!-- UAV0 -->
<group ns="uav0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="1"/>
<arg name="fcu_url" default="udp://:14550@localhost:14555"/>
<!-- MAVROS -->
<include file="$(find mavros)/launch/apm.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(arg ID)"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- UAV1 -->
<group ns="uav1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="2"/>
<arg name="fcu_url" default="udp://:14560@localhost:14565"/>
<!-- MAVROS -->
<include file="$(find mavros)/launch/apm.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(arg ID)"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
</launch>
<!-- to add more UAVs:
Increase the id
Change the name space
Set the FCU to default="udp://:14540+id*10@localhost:14545+id*10" -->
57 changes: 57 additions & 0 deletions libraries/SITL/examples/Airsim/multi_vehicle.sh
@@ -0,0 +1,57 @@
#!/bin/bash

# Example script for multi-vehicle simulation with AirSim and usage with ROS
# see http://ardupilot.org/dev/docs/sitl-with-airsim.html#multi-vehicle-simulation for details

# Usage - From ardupilot root directory, run - libraries/SITL/examples/Airsim/multi_vehicle.sh $GCS_IP
# $GCS_IP is the IP address of the system running the GCS, by default is 127.0.0.1

# Kill all SITL binaries when exiting
trap "killall -9 arducopter" SIGINT SIGTERM EXIT

# assume we start the script from the root directory
ROOTDIR=$PWD
COPTER=$ROOTDIR/build/sitl/bin/arducopter

# Set GCS_IP address
if [ -z $1 ]; then
GCS_IP="127.0.0.1"
else
GCS_IP=$1
fi


BASE_DEFAULTS="$ROOTDIR/Tools/autotest/default_params/copter.parm,$ROOTDIR/libraries/SITL/examples/Airsim/quadX.parm"

[ -x "$COPTER" ] || {
./waf configure --board sitl
./waf copter
}

# start up main copter in the current directory
$COPTER --model airsim-copter --uartA udpclient:$GCS_IP:14550 --uartB tcp:0 --defaults $BASE_DEFAULTS &

# Set number of "extra" copters to be simulated, change this for increasing the count
NCOPTERS="1"


# now start another copter, using
# a separate directory to keep the eeprom.bin and logs separate
for i in $(seq $NCOPTERS); do
echo "Starting copter $i"
mkdir -p copter$i

SYSID=$(expr $i + 1)
UDP_PORT=$((14550 + $i * 10))

# create seperate parameter file for each drone for SYSID
cat <<EOF > copter$i/identity.parm
SYSID_THISMAV $SYSID
EOF

pushd copter$i
$COPTER --model airsim-copter --uartA udpclient:$GCS_IP:$UDP_PORT --uartB tcp:$i \
--instance $i --defaults $BASE_DEFAULTS,identity.parm &
popd
done
wait