Closed Seiwert closed 6 years ago
Check that you have different udp ports pair used for each autopilot. Mavros do not fully support multipoint comm.
Also /diagnostics
needed to be for all three nodes. VER responses should be only one per sysid/compid (but fcu may send that message at startup).
@vooon I am fairly certain I have separate UDP ports set correctly. Here is my launch file:
<launch>
<!-- MAVROS posix SITL environment launch script -->
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<arg name="est" default="ekf2"/>
<arg name="vehicle" default="iris"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
<arg name="headless" default="false"/>
<arg name="gui" default="true"/>
<arg name="ns" default="/"/>
<arg name="pluginlists_yaml" default="$(find mavros)/launch/px4_pluginlists.yaml" />
<arg name="config_yaml" default="$(find mavros)/launch/px4_config.yaml" />
<!-- Load world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="headless" value="$(arg headless)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="world_name" value="$(arg world)" />
<arg name="debug" value="$(arg debug)" />
<arg name="verbose" value="$(arg verbose)" />
<arg name="paused" value="$(arg paused)" />
</include>
<!-- UAV1 iris_1-->
<group ns="uav1">
<arg name="fcu_url" default="udp://:14540@localhost:14557"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="1"/>
<arg name="tgt_component" value="1"/>
<arg name="rcS1" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_1"/>
<arg name="ID" value="1"/>
<include file="$(find px4)/launch/single_vehcile_spawn.launch">
<arg name="x" value="0"/>
<arg name="y" value="0"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="rcS" value="$(arg rcS1)"/>
<arg name="mavlink_udp_port" value="14560"/>
<arg name="ID" value="$(arg ID)"/>
</include>
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
<arg name="config_yaml" value="$(arg config_yaml)" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
</include>
</group>
<!-- UAV2 iris_2 -->
<group ns="uav2">
<arg name="fcu_url" default="udp://:14541@localhost:14559"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="2"/>
<arg name="tgt_component" value="2"/>
<arg name="rcS2" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_2"/>
<arg name="ID" value="2"/>
<include file="$(find px4)/launch/single_vehcile_spawn.launch">
<arg name="x" value="1"/>
<arg name="y" value="0"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="rcS" value="$(arg rcS2)"/>
<arg name="mavlink_udp_port" value="14562"/>
<arg name="ID" value="$(arg ID)"/>
</include>
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
<arg name="config_yaml" value="$(arg config_yaml)" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
</include>
</group>
<!-- UAV2 iris_3 -->
<group ns="uav3">
<arg name="fcu_url" default="udp://:14542@localhost:14561"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="3"/>
<arg name="tgt_component" value="3"/>
<arg name="rcS3" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_3"/>
<arg name="ID" value="3"/>
<include file="$(find px4)/launch/single_vehcile_spawn.launch">
<arg name="x" value="1"/>
<arg name="y" value="1"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="rcS" value="$(arg rcS3)"/>
<arg name="mavlink_udp_port" value="14564"/>
<arg name="ID" value="$(arg ID)"/>
</include>
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
<arg name="config_yaml" value="$(arg config_yaml)" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
</include>
</group>
</launch>
<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
As you can see, the _fcuurl is different for each UAV. If I'm understanding this all correctly, those variables will be passed into the mavros node.launch file and should bring up separate instances of MAVROS looking at those ports.
I've also modified my iris_1 - 3 files to match these UDP ports along with the iris_1-3.urdf files.
Thanks for taking a look at this.
Also, I apologize, I forgot to upload diagnostic data from all three. Here is that information:
header:
seq: 104
stamp:
secs: 99
nsecs: 0
frame_id: ''
status:
-
level: 0
name: uav2/mavros: FCU connection
message: connected
hardware_id: udp://:14541@localhost:14559
values:
-
key: Received packets:
value: 40730
-
key: Dropped packets:
value: 0
-
key: Buffer overruns:
value: 0
-
key: Parse errors:
value: 0
-
key: Rx sequence number:
value: 28
-
key: Tx sequence number:
value: 130
-
key: Rx total bytes:
value: 1693902
-
key: Tx total bytes:
value: 51247
-
key: Rx speed:
value: 17294.000000
-
key: Tx speed:
value: 517.000000
-
level: 0
name: uav2/mavros: GPS
message: 3D fix
hardware_id: udp://:14541@localhost:14559
values:
-
key: Satellites visible
value: 10
-
key: Fix type
value: 3
-
key: EPH (m)
value: 0.00
-
key: EPV (m)
value: 0.00
-
level: 0
name: uav2/mavros: Heartbeat
message: Normal
hardware_id: udp://:14541@localhost:14559
values:
-
key: Heartbeats since startup
value: 97
-
key: Frequency (Hz)
value: 1.000000
-
key: Vehicle type
value: Quadrotor
-
key: Autopilot type
value: PX4 Autopilot
-
key: Mode
value: AUTO.TAKEOFF
-
key: System status
value: Active
-
level: 0
name: uav2/mavros: System
message: Normal
hardware_id: udp://:14541@localhost:14559
values:
-
key: Sensor present
value: 0x00000000
-
key: Sensor enabled
value: 0x00000000
-
key: Sensor helth
value: 0x00000000
-
key: CPU Load (%)
value: 0.0
-
key: Drop rate (%)
value: 0.0
-
key: Errors comm
value: 0
-
key: Errors count #1
value: 0
-
key: Errors count #2
value: 0
-
key: Errors count #3
value: 0
-
key: Errors count #4
value: 0
-
level: 0
name: uav2/mavros: Battery
message: Normal
hardware_id: udp://:14541@localhost:14559
values:
-
key: Voltage
value: 10.05
-
key: Current
value: -1.0
-
key: Remaining
value: 17.0
-
level: 0
name: uav2/mavros: Time Sync
message: Normal
hardware_id: udp://:14541@localhost:14559
values:
-
key: Timesyncs since startup
value: 829
-
key: Frequency (Hz)
value: 10.000000
-
key: Last dt (ms)
value: -0.038142
-
key: Mean dt (ms)
value: -0.137742
-
key: Last system time (s)
value: 97.724807000
-
key: Time offset (s)
value: 1.175177743
---
header:
seq: 104
stamp:
secs: 99
nsecs: 0
frame_id: ''
status:
-
level: 0
name: uav3/mavros: FCU connection
message: connected
hardware_id: udp://:14542@localhost:14561
values:
-
key: Received packets:
value: 40522
-
key: Dropped packets:
value: 0
-
key: Buffer overruns:
value: 0
-
key: Parse errors:
value: 0
-
key: Rx sequence number:
value: 76
-
key: Tx sequence number:
value: 125
-
key: Rx total bytes:
value: 1685315
-
key: Tx total bytes:
value: 51127
-
key: Rx speed:
value: 17294.000000
-
key: Tx speed:
value: 517.000000
-
level: 0
name: uav3/mavros: GPS
message: 3D fix
hardware_id: udp://:14542@localhost:14561
values:
-
key: Satellites visible
value: 10
-
key: Fix type
value: 3
-
key: EPH (m)
value: 0.00
-
key: EPV (m)
value: 0.00
-
level: 0
name: uav3/mavros: Heartbeat
message: Normal
hardware_id: udp://:14542@localhost:14561
values:
-
key: Heartbeats since startup
value: 97
-
key: Frequency (Hz)
value: 1.000000
-
key: Vehicle type
value: Quadrotor
-
key: Autopilot type
value: PX4 Autopilot
-
key: Mode
value: AUTO.TAKEOFF
-
key: System status
value: Active
-
level: 0
name: uav3/mavros: System
message: Normal
hardware_id: udp://:14542@localhost:14561
values:
-
key: Sensor present
value: 0x00000000
-
key: Sensor enabled
value: 0x00000000
-
key: Sensor helth
value: 0x00000000
-
key: CPU Load (%)
value: 0.0
-
key: Drop rate (%)
value: 0.0
-
key: Errors comm
value: 0
-
key: Errors count #1
value: 0
-
key: Errors count #2
value: 0
-
key: Errors count #3
value: 0
-
key: Errors count #4
value: 0
-
level: 0
name: uav3/mavros: Battery
message: Normal
hardware_id: udp://:14542@localhost:14561
values:
-
key: Voltage
value: 10.56
-
key: Current
value: -1.0
-
key: Remaining
value: 43.0
-
level: 0
name: uav3/mavros: Time Sync
message: Normal
hardware_id: udp://:14542@localhost:14561
values:
-
key: Timesyncs since startup
value: 829
-
key: Frequency (Hz)
value: 10.000000
-
key: Last dt (ms)
value: -0.195105
-
key: Mean dt (ms)
value: -0.062807
-
key: Last system time (s)
value: 97.279937000
-
key: Time offset (s)
value: 1.619984958
---
header:
seq: 104
stamp:
secs: 99
nsecs: 0
frame_id: ''
status:
-
level: 0
name: uav1/mavros: FCU connection
message: connected
hardware_id: udp://:14540@localhost:14557
values:
-
key: Received packets:
value: 41327
-
key: Dropped packets:
value: 0
-
key: Buffer overruns:
value: 0
-
key: Parse errors:
value: 0
-
key: Rx sequence number:
value: 113
-
key: Tx sequence number:
value: 117
-
key: Rx total bytes:
value: 1712725
-
key: Tx total bytes:
value: 50904
-
key: Rx speed:
value: 17294.000000
-
key: Tx speed:
value: 517.000000
-
level: 0
name: uav1/mavros: GPS
message: 3D fix
hardware_id: udp://:14540@localhost:14557
values:
-
key: Satellites visible
value: 10
-
key: Fix type
value: 3
-
key: EPH (m)
value: 0.00
-
key: EPV (m)
value: 0.00
-
level: 0
name: uav1/mavros: Heartbeat
message: Normal
hardware_id: udp://:14540@localhost:14557
values:
-
key: Heartbeats since startup
value: 97
-
key: Frequency (Hz)
value: 1.000000
-
key: Vehicle type
value: Quadrotor
-
key: Autopilot type
value: PX4 Autopilot
-
key: Mode
value: AUTO.TAKEOFF
-
key: System status
value: Active
-
level: 0
name: uav1/mavros: System
message: Normal
hardware_id: udp://:14540@localhost:14557
values:
-
key: Sensor present
value: 0x00000000
-
key: Sensor enabled
value: 0x00000000
-
key: Sensor helth
value: 0x00000000
-
key: CPU Load (%)
value: 0.0
-
key: Drop rate (%)
value: 0.0
-
key: Errors comm
value: 0
-
key: Errors count #1
value: 0
-
key: Errors count #2
value: 0
-
key: Errors count #3
value: 0
-
key: Errors count #4
value: 0
-
level: 0
name: uav1/mavros: Battery
message: Normal
hardware_id: udp://:14540@localhost:14557
values:
-
key: Voltage
value: 10.17
-
key: Current
value: -1.0
-
key: Remaining
value: 23.0
-
level: 0
name: uav1/mavros: Time Sync
message: Normal
hardware_id: udp://:14540@localhost:14557
values:
-
key: Timesyncs since startup
value: 829
-
key: Frequency (Hz)
value: 10.000000
-
key: Last dt (ms)
value: 0.275454
-
key: Mean dt (ms)
value: -0.096912
-
key: Last system time (s)
value: 97.498780000
-
key: Time offset (s)
value: 1.401330181
So it looks like you took the launch offered by PX4 and tried to add a third UAV without understanding the port configuration.
Looking at this chuck of output you have port overlap:
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14556 remote port 14550
INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14557 remote port 14540
INFO [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
INFO [logger] logger started (mode=all)pxh> INFO [simulator] Got initial simulation data, running sim..
INFO [pwm_out_sim] MODE_8PWM
INFO [tone_alarm] startup
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14563 remote port 14550
INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14561 remote port 14542
INFO [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
INFO [logger] logger started (mode=all)
pxh> INFO [simulator] Got initial simulation data, running sim..
INFO [pwm_out_sim] MODE_8PWM
INFO [tone_alarm] startup
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14558 remote port 14550
INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14559 remote port 14541
I'd reconfigure these ports (both in the launch and gazebo files). To make this less confusing: use the default for iris_1. For iris_2 increase each port value by 10, and for iris_3 increase each by another 10.
Good luck.
EDIT: For example when I launch 3 UAVs I have:
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14421 remote port 14427
INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14422 remote port 14423
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14411 remote port 14417
INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14412 remote port 14413
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14401 remote port 14407
INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14402 remote port 14403
Why you also change tgt_component
? You only change system-id of FCU, so you should have: 1.1, 2.1, 3.1.
good point @vooon... @Seiwert look over these again: https://github.com/PX4/Firmware/tree/master/posix-configs/SITL https://github.com/PX4/Firmware/blob/master/launch/multi_uav_mavros_sitl.launch https://github.com/PX4/Firmware/blob/master/launch/single_vehcile_spawn.launch https://github.com/PX4/Firmware/blob/master/posix-configs/SITL/init/ekf2/multiple_iris
@vooon @lamping7
The issue was the tgt_component
I changed them back to one and that worked. Thank you both so much for all your help. And @lamping7 , thanks for those resources. I'm new to this so that's why I changed that variable. I'll read up on what you sent and hopefully not make similar mistakes in the future.
@Seiwert can you please update the docs on the PX4 devguide so it reflects what was done here? Cause currently there's a lot of people struggling to get this running. Thanks in advance
Also note: checkid
may help with ID problems. By default it subscribe to mavlink/from
, so for your launch only what needed to do is select namespace.
rosrun mavros checkid __ns:=/uav1
@TSC21 I'd be happy to. From my experience though, the docs aren't too far off. The main issue I had was with the single_vehcile_spawn.launch
(yes that typo is what it's actually named). The issue in that file dealt with a bug in xacro.py
. It appears as though that program is incorrectly traversing subdirectories and never finds iris_base.xacro
The way I got around that is by manually running xacro.py
and changing the arguments passed into it so it finds the .xacro file. I also ended up modifying single_vehcile_spawn.launch
like so:
<launch>
<!-- Posix SITL environment launch script -->
<arg name="x" default="0"/>
<arg name="y" default="0"/>
<arg name="z" default="0"/>
<arg name="R" default="0"/>
<arg name="P" default="0"/>
<arg name="Y" default="0"/>
<arg name="est" default="lpe"/>
<arg name="vehicle" default="iris"/>
<arg name="ID" default="1"/>
<arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_$(arg ID)"/>
<arg name="mavlink_udp_port" default="14560" />
<!--
<arg name="cmd" default="$(find xacro)/xacro.py $(find px4)/Tools/sitl_gazebo/models/rotors_description/urdf/$(arg vehicle)_base.xacro rotors_description_dir:=$(find px4)/Tools/sitl_gazebo/models/rotors_description mavlink_udp_port:=$(arg mavlink_udp_port) > $(arg vehicle)_$(arg ID).urdf ; 'gz sdf -p $(arg vehicle)_$(arg ID).urdf'" />
-->
<arg name="cmd" default="gz sdf -p $(arg vehicle)_$(arg ID).urdf"/>
<param command="$(arg cmd)" name="$(arg vehicle)_$(arg ID)_sdf" />
<node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen"
args="$(find px4) $(arg rcS)">
</node>
<node name="$(arg vehicle)_$(arg ID)_spawn" output="screen" pkg="gazebo_ros" type="spawn_model"
args="-sdf -param $(arg vehicle)_$(arg ID)_sdf -model $(arg vehicle)_$(arg ID) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)" respawn="false"/>
</launch>
<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
Then, I just made copies of the original urdf file and modified them manually as needed. As you can see, this isn't an ideal method to document. So, my question for you, @TSC21 , is xacro.py
maintained by PX4 (or does a certain version ship with PX4)? If that's the case, it seems as though that needs updated and not the docs.
I can provide more details later and perhaps do a pull request but I thought I'd let you know in case you know more about xacro
.
It actually is used here: https://github.com/PX4/sitl_gazebo/blob/master/CMakeLists.txt#L174-L184, to generate the Iris model.
Maybe you can add a way of generating those the same way using the CMakeLists.txt
capabilities.
@TSC21 I'll take a look at that. For clarification, here is the original error:
Usage: xacro.py [options] <input>
xacro.py: error: expected exactly one input file as argument
while processing /home/aseiwert/src/Firmware/launch/ORIGINAL_single_vehcile_spawn.launch:
Invalid <param> tag: Cannot load command parameter [iris_1_sdf]: command [/opt/ros/kinetic/share/xacro/xacro.py /home/aseiwert/src/Firmware/Tools/sitl_gazebo/models/rotors_description/urdf/iris_base.xacro rotors_description_dir:=/home/aseiwert/src/Firmware/Tools/sitl_gazebo/models/rotors_description mavlink_udp_port:=14560 > iris_1.urdf ; 'gz sdf -p iris_1.urdf'] returned with code [2].
Param xml is <param command="$(arg cmd)" name="$(arg vehicle)_$(arg ID)_sdf"/>
The traceback for the exception was written to the log file
Reviewing this again though I don't think the error is with xacro after all. It looks like it's an issue with the concatenation on the 'cmd' argument in the launch file.
@TSC21 Okay... I kind of fixed it...
Turns out xacro is fine, so you were right about that. It's something wrong with the launch file incorrectly parsing the command and it's given arguments. If you copy what it tries to run and run it in a separate terminal it works fine. I don't know enough about launch files to say why that is the case.
Anyway, here is what I did:
workaround.sh
(put it in /src/Firmware/):
# $1 = find px4
# $2 = vehicle
# $3 = mavlink_udp_port
# $4 = ID
rosrun xacro xacro $1/Tools/sitl_gazebo/models/rotors_description/urdf/$2_base.xacro rotors_description_dir:=$1/Tools/sitl_gazebo/models/rotors_description mavlink_udpport:=$3 > $2$4.urdf
2. Then change the `single_vehcile_spawn.launch` to be this:
```xml
<launch>
<!-- Posix SITL environment launch script -->
<arg name="x" default="0"/>
<arg name="y" default="0"/>
<arg name="z" default="0"/>
<arg name="R" default="0"/>
<arg name="P" default="0"/>
<arg name="Y" default="0"/>
<arg name="est" default="lpe"/>
<arg name="vehicle" default="iris"/>
<arg name="ID" default="1"/>
<arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_$(arg ID)"/>
<arg name="mavlink_udp_port" default="14560" />
<param name="launchWorkAround" command="rosrun px4 workaround.sh $(find px4) $(arg vehicle) $(arg mavlink_udp_port) $(arg ID)" />
<arg name="cmd" default="gz sdf -p $(arg vehicle)_$(arg ID).urdf" />
<param command="$(arg cmd)" name="$(arg vehicle)_$(arg ID)_sdf" />
<node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen"
args="$(find px4) $(arg rcS)">
</node>
<node name="$(arg vehicle)_$(arg ID)_spawn" output="screen" pkg="gazebo_ros" type="spawn_model"
args="-sdf -param $(arg vehicle)_$(arg ID)_sdf -model $(arg vehicle)_$(arg ID) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)" respawn="false"/>
</launch>
<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
This is definitely not the right answer but it is something I suppose...
I doubt it is pull worthy for PX4 but let me know what you think.
Issue details
Multiple UAV control is only working with one UAV. (Example: I can arm "uav1" through Mavros but I am unable to arm "uav2" or "uav3")
However, I can query info from all UAV's (e.g. GPS data) and QGroundControl is able to communicate and control all UAV's as well. My setup is in simulation and is based off of these instructions.
MAVROS version and platform
Mavros: 0.21.4 ROS: Kinetic Ubuntu: 16.04
Autopilot type and version
[ ] ArduPilot [X] PX4
Version: 1.0.0
Node logs
Diagnostics
Check ID
Additional Details
Here is a snippet of some code I'm running to try to arm a uav. This works perfectly fine with uav1 but does not work with uav2 and uav3. I've also gotten to the point where I can set uav1 to OFFBOARD and command it in software but that same code does not work with uav2 and uav3. I've also modified my launch file to just launch uav2 or uav3 by themselves but that does not work either.
I'm at a loss for what's wrong with my setup so any help would be greatly appreciated. Thank you for your time.