Mavros for multiple drones with obstacle avoidance (px4-avoidance)

[pooyanght]

---

Issue details

I want to use px4_avoidance for multiple drones. I have updated the model and launch files regarding that, and when I run everything, I can see the point clouds on rvis, and the drones on the environment, however only one drone has the obstacle avoidance feature and the other drone does not. It is strange that when I change the first drone to a simple drone (IRIS), still I can see the obstacle data on the first drone (not the second drone) on QGC. I can see that the obstacle data is being sent to the Mavros, but I don't know why it seems not being used for the second drone!

I have used different groups, and different IPs for each drone, and I have noticed a repository for mavros update #1967 which should handle mavros for multiple drones. Although I have used it's instruction for updating my code regarding that, the px4_avoidance repository uses the old frame ids in it's px4_confige.yaml (FCU and local_origin) which were changed to the map, odom, and base_link (having said that still it works for one drone).

Do you have any suggestions about what needs to be updated?

each group is like follow:

`

<arg name="est" default="ekf2"/>

<env name="PX4_SIM_MODEL" value="gazebo-classic_$(arg vehicle)" />
<!-- <env name="PX4_SIM_MODEL" value="gazebo-classic_iris_obs_avoid" /> -->
<arg name="mavlink_udp_port" value="14560"/>
<arg name="mavlink_tcp_port" value="4560"/>
<arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
<arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
<arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
<arg name="mavlink_id" value="$(eval 1 + arg('ID'))" />

<!-- Define a static transform from a camera internal frame to the fcu for every camera used -->

<node pkg="tf" type="static_transform_publisher" name="tf_depth_camera_drone1"
        args="0 0 0 -1.57 0 -1.57 fcu camera_link_drone1 10"/>

<!-- PX4 configs -->
<arg name="interactive" default="true"/>
<!-- generate sdf vehicle model -->
<arg name="cmd" default="$(find mavlink_sitl_gazebo)/scripts/jinja_gen.py --stdout --mavlink_id=$(arg mavlink_id) --mavlink_udp_port=$(arg mavlink_udp_port) --mavlink_tcp_port=$(arg mavlink_tcp_port) --gst_udp_port=$(arg gst_udp_port) --video_uri=$(arg video_uri) --mavlink_cam_udp_port=$(arg mavlink_cam_udp_port) $(find mavlink_sitl_gazebo)/models/$(arg model)/$(arg model).sdf.jinja $(find mavlink_sitl_gazebo)"/>
<param command="$(arg cmd)" name="sdf_$(arg vehicle)$(arg ID)"/>
<!-- PX4 SITL -->
<arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>
<arg     if="$(arg interactive)" name="px4_command_arg1" value="-d"/>
<node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/build/px4_sitl_default/etc -s etc/init.d-posix/rcS -i $(arg ID) -w sitl_$(arg vehicle)_$(arg ID) $(arg px4_command_arg1)">
</node>

<!-- spawn vehicle -->
<node name="$(anon vehicle_spawn)" pkg="gazebo_ros" type="spawn_model" output="screen" args="-sdf -param sdf_$(arg vehicle)$(arg ID) -model $(arg vehicle)$(arg ID) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>

<!-- Launch MavROS -->
<include file="$(find mavros)/launch/node.launch">
    <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" /> 
    <arg name="config_yaml" value="$(find avoidance)/resource/px4_config_drone11.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)" />

    <arg name="base_link_frame_id" value="$(arg base_link_frame_id)" />
    <arg name="odom_frame_id" value="$(arg odom_frame_id)" />
    <arg name="map_frame_id" value="$(arg map_frame_id)" />

</include>

<!-- Launch local planner-Depth camera -->
<arg name="manager"             default="local_planner_manager"/>

<node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="local_planner_nodelet" args="load LocalPlannerNodelet $(arg manager)" output="screen">
  <param name="goal_x_param" value="17" />
  <param name="goal_y_param" value="15"/>
  <param name="goal_z_param" value="3" />
  <param name="world_name" value="$(find avoidance)/sim/worlds/$(arg world_file_name).yaml" />
  <rosparam param="pointcloud_topics" subst_value="True">[/iris_obs_avoid_drone10/camera/depth/points]</rosparam>
</node>

`

MAVROS version and platform

Mavros: 1.20.0
ROS: Noetic
Ubuntu: 20 (wsl2)

Autopilot type and version

[ ] ArduPilot
[ * ] PX4

Version: v1.15.0

Node logs

[ERROR] [1730492653.392307372, 183.716000000]: ODOM: Ex: "base_link" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1730492653.984864976, 184.308000000]: ODOM: Ex: "base_link" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1730492654.394022015, 184.716000000]: ODOM: Ex: "base_link" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1730492654.985158429, 185.308000000]: ODOM: Ex: "base_link" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1730492655.393429968, 185.716000000]: ODOM: Ex: "base_link" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1730492656.017294071, 186.340000000]: ODOM: Ex: "base_link" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1730492656.394721017, 186.716000000]: ODOM: Ex: "base_link" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1730492657.018012937, 187.340000000]: ODOM: Ex: "base_link" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1730492657.394195543, 187.716000000]: ODOM: Ex: "base_link" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1730492658.019291637, 188.340000000]: ODOM: Ex: "base_link" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1730492658.426839664, 188.748000000]: ODOM: Ex: "base_link" passed to lookupTransform argument target_frame does not exist.

Diagnostics

place here result of:
rostopic echo -n1 /diagnostics

header:
  seq: 277
  stamp:
    secs: 271
    nsecs:         0
  frame_id: ''
status:
  -
    level: 0
    name: "drone2/mavros: FCU connection"
    message: "connected"
    hardware_id: "udp://:14541@localhost:14581"
    values:
      -
        key: "Received packets:"
        value: "51229"
      -
        key: "Dropped packets:"
        value: "0"
      -
        key: "Buffer overruns:"
        value: "0"
      -
        key: "Parse errors:"
        value: "0"
      -
        key: "Rx sequence number:"
        value: "29"
      -
        key: "Tx sequence number:"
        value: "0"
      -
        key: "Rx total bytes:"
        value: "7175690"
      -
        key: "Tx total bytes:"
        value: "1028881"
      -
        key: "Rx speed:"
        value: "26580.000000"
      -
        key: "Tx speed:"
        value: "3802.000000"
  -
    level: 0
    name: "drone2/mavros: GPS"
    message: "3D fix"
    hardware_id: "udp://:14541@localhost:14581"
    values:
      -
        key: "Satellites visible"
        value: "10"
      -
        key: "Fix type"
        value: "3"
      -
        key: "EPH (m)"
        value: "0.00"
      -
        key: "EPV (m)"
        value: "0.00"
  -
    level: 1
    name: "drone2/mavros: Mount"
    message: "Can not diagnose in this targeting mode"
    hardware_id: "udp://:14541@localhost:14581"
    values:
      -
        key: "Mode"
        value: "255"
  -
    level: 0
    name: "drone2/mavros: Heartbeat"
    message: "Normal"
    hardware_id: "udp://:14541@localhost:14581"
    values:
      -
        key: "Heartbeats since startup"
        value: "270"
      -
        key: "Frequency (Hz)"
        value: "1.000000"
      -
        key: "Vehicle type"
        value: "Quadrotor"
      -
        key: "Autopilot type"
        value: "PX4 Autopilot"
      -
        key: "Mode"
        value: "AUTO.LOITER"
      -
        key: "System status"
        value: "Uninit"
  -
    level: 2
    name: "drone2/mavros: System"
    message: "Sensor health"
    hardware_id: "udp://:14541@localhost:14581"
    values:
      -
        key: "Sensor present"
        value: "0x0100C02D"
      -
        key: "Sensor enabled"
        value: "0x0101C02D"
      -
        key: "Sensor health"
        value: "0x2320C83F"
      -
        key: "3D gyro"
        value: "Ok"
      -
        key: "3D magnetometer"
        value: "Ok"
      -
        key: "absolute pressure"
        value: "Ok"
      -
        key: "GPS"
        value: "Ok"
      -
        key: "x/y position control"
        value: "Ok"
      -
        key: "motor outputs / control"
        value: "Ok"
      -
        key: "rc receiver"
        value: "Fail"
      -
        key: "Logging"
        value: "Ok"
      -
        key: "CPU Load (%)"
        value: "12.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: "drone2/mavros: Battery"
    message: "Normal"
    hardware_id: "udp://:14541@localhost:14581"
    values:
      -
        key: "Voltage"
        value: "16.20"
      -
        key: "Current"
        value: "1.0"
      -
        key: "Remaining"
        value: "100.0"
  -
    level: 0
    name: "drone2/mavros: Time Sync"
    message: "Normal"
    hardware_id: "udp://:14541@localhost:14581"
    values:
      -
        key: "Timesyncs since startup"
        value: "175"
      -
        key: "Frequency (Hz)"
        value: "10.000000"
      -
        key: "Last RTT (ms)"
        value: "0.000000"
      -
        key: "Mean RTT (ms)"
        value: "0.091428"
      -
        key: "Last remote time (s)"
        value: "270.972000000"
      -
        key: "Estimated time offset (s)"
        value: "312947730.358039021"
---
pooyan@COT-BinAlien8:~/Firmware$ rostopic echo -n1 /diagnostics
header:
  seq: 291
  stamp:
    secs: 285
    nsecs:         0
  frame_id: ''
status:
  -
    level: 0
    name: "drone1/mavros: FCU connection"
    message: "connected"
    hardware_id: "udp://:14540@localhost:14580"
    values:
      -
        key: "Received packets:"
        value: "57314"
      -
        key: "Dropped packets:"
        value: "0"
      -
        key: "Buffer overruns:"
        value: "0"
      -
        key: "Parse errors:"
        value: "0"
      -
        key: "Rx sequence number:"
        value: "226"
      -
        key: "Tx sequence number:"
        value: "0"
      -
        key: "Rx total bytes:"
        value: "7550843"
      -
        key: "Tx total bytes:"
        value: "1082789"
      -
        key: "Rx speed:"
        value: "26586.000000"
      -
        key: "Tx speed:"
        value: "3802.000000"
  -
    level: 0
    name: "drone1/mavros: GPS"
    message: "3D fix"
    hardware_id: "udp://:14540@localhost:14580"
    values:
      -
        key: "Satellites visible"
        value: "10"
      -
        key: "Fix type"
        value: "3"
      -
        key: "EPH (m)"
        value: "0.00"
      -
        key: "EPV (m)"
        value: "0.00"
  -
    level: 1
    name: "drone1/mavros: Mount"
    message: "Can not diagnose in this targeting mode"
    hardware_id: "udp://:14540@localhost:14580"
    values:
      -
        key: "Mode"
        value: "255"
  -
    level: 0
    name: "drone1/mavros: Heartbeat"
    message: "Normal"
    hardware_id: "udp://:14540@localhost:14580"
    values:
      -
        key: "Heartbeats since startup"
        value: "284"
      -
        key: "Frequency (Hz)"
        value: "1.000000"
      -
        key: "Vehicle type"
        value: "Quadrotor"
      -
        key: "Autopilot type"
        value: "PX4 Autopilot"
      -
        key: "Mode"
        value: "AUTO.LOITER"
      -
        key: "System status"
        value: "Standby"
  -
    level: 2
    name: "drone1/mavros: System"
    message: "Sensor health"
    hardware_id: "udp://:14540@localhost:14580"
    values:
      -
        key: "Sensor present"
        value: "0x2100C02D"
      -
        key: "Sensor enabled"
        value: "0x2101C02D"
      -
        key: "Sensor health"
        value: "0x3320C83F"
      -
        key: "3D gyro"
        value: "Ok"
      -
        key: "3D magnetometer"
        value: "Ok"
      -
        key: "absolute pressure"
        value: "Ok"
      -
        key: "GPS"
        value: "Ok"
      -
        key: "x/y position control"
        value: "Ok"
      -
        key: "motor outputs / control"
        value: "Ok"
      -
        key: "rc receiver"
        value: "Fail"
      -
        key: "Logging"
        value: "Ok"
      -
        key: "Avoidance/collision prevention"
        value: "Ok"
      -
        key: "CPU Load (%)"
        value: "12.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: "drone1/mavros: Battery"
    message: "Normal"
    hardware_id: "udp://:14540@localhost:14580"
    values:
      -
        key: "Voltage"
        value: "16.20"
      -
        key: "Current"
        value: "1.0"
      -
        key: "Remaining"
        value: "100.0"
  -
    level: 0
    name: "drone1/mavros: Time Sync"
    message: "Normal"
    hardware_id: "udp://:14540@localhost:14580"
    values:
      -
        key: "Timesyncs since startup"
        value: "326"
      -
        key: "Frequency (Hz)"
        value: "10.000000"
      -
        key: "Last RTT (ms)"
        value: "0.000000"
      -
        key: "Mean RTT (ms)"
        value: "0.122699"
      -
        key: "Last remote time (s)"
        value: "284.928000000"
      -
        key: "Estimated time offset (s)"
        value: "166794893.552181780"
---

Check ID

rosrun mavros checked

I do have different target system ids but still get this error!

WARNING: mavros/target_system_id not set. Used default value: 1
WARNING: mavros/target_component_id not set. Used default value: 1
NOTE: Target parameters may be unset, but that may be result of incorrect --mavros-ns option.Double check results!
ERROR. I got 0 addresses, but not your target 1:1

---
Received 0 messages, from 0 addresses
sys:comp   list of messages

[pooyanght]

As an update on this, I have implemented the new frame_ids, drone1/map, drone1/base_link, and drone/odom, and the same for the second drone. I used the latest px4_confige.yaml provided by the MAVROS. So, I have the following frame id tree:

This shows that the frame_ids are well separated, however, I still have the same problem that the avoidance only works on the first drone and for the second drone, although I can see the point clouds, on the QGC I can't see them and if I change the drone type for the first drone to a simple IRIS, still I can see some obstacle ranges on QGC for the first drone which seems to come from the second drone!

Does anyone have any idea about what might be wrong? The other thing is that there are connections between drone1/mavros and drone2/mavros nodes, plus some connection between drone1/tf and drone2/tf related nodes which come from a default usage of these nodes.