Loading multiple drones (px4 iris models) in a gazebo-classic simulation and controlling them using XRCE-DDS with ROS2 Foxy

PX4 Autopilot
1

Hello! I am trying to create a gazebo simulation with multiple drones that I want to fly to different positions in a gazebo world to simulate a surveying task. However, I have been struggling to load and control multiple drones in a gazebo world. I am using the PX4-Autopilot firmware, gazebo-classic 11 and ROS2 foxy running on ubuntu 20.04, and I am using the XRCE-DDS bridge for the client-agent communication. I am able to load one iris model in a gazebo world by running make px4_sitl gazebo-classic under the PX4-Autopilot directory, and I can control it by starting an XRCE-DDS agent and running some example nodes in the px4_ros_com package found on GitHub - PX4/px4_ros_com: ROS2/ROS interface with PX4 through a Fast-RTPS bridge (ex: I can run the offboard_control node successfully). I can also load multiple drones (px4 iris models) in a gazebo world by running ./Tools/simulation/gazebo-classic/sitl_multiple_run.sh -t px4_sitl_rtps m iris n 2 under the PX4-Autopilot directory, however I can’t figure out how to interface them with ros2 in order to control them. These are a few questions I have about this:

How can I control multiple iris models using ros2 nodes after loading them in a gazebo world using the aforementioned ./Tools/simulation/gazebo-classic/sitl_multiple_run.sh t px4_sitl_rtps m iris n 2 command? How can I load multiple px4 iris models in a custom gazebo-classic world? Do I need to start multiple micro-XRCE agents in order to control multiple drones in a Gazebo-classic simulation using ros2 nodes? Any help about these questions would be greatly appreciated, even if it’s directing me to the right resources/documentations. I am relatively new to using ROS2 with gazebo for px4 drones simulations, so I know these questions might be trivial, but I am kinda stuck and any help would very useful. Thank you!

2

Hi @Terence-I , Please take a look at

and

it should have all answers to your questions.

Just to be sure, you are using the main PX4 branch or one of the v1.14-beta tags, right?

1 Like
3

HI @Benja, thank you so much for a quick response and yes I’m using the main PX4 branch. The resources you shared were very helpful, I was able to control multiple iris models in a gazebo world using QgroundControl. From the documentations you shared, I read that each PX4 instance (i.e., each iris model) receives a unique px4_instance number/id after running the sitl_multiple_run.sh file, and I was wondering how to send vehicle position or velocity commands to a specific iris model using its respective px4_instance number. For example, is there a way to specify the px4_instance number of the iris drone you want to control when publishing velocity commands to the velocity topic using the ros2 topic pub command? Is there a way to specify the px4_instance number of the iris model you want to control when writing a ROS2 node? Thank again for your help, I really appreciate it!

4

Hello Terence,

When the bridge between PX4 and ROS2 is running, it will automatically mirror some of the uOrb topics as ROS2 topics. When multiple instances of the PX4 are running, each instance has its ROS2 namespace, meaning all the topics created are prefixed by /<namespace>. For example, when I run two PX4 instances in Gazebo while the bridge is running, I get the following :

image
image611×909 107 KB

If you want to control a specific instance from a ROS2 node, you have to publish to its specific topic. Example in c++ (this code is inside the constructor of a class inheriting rclcpp::Node) :

            // Create publishers
            this->offboard_control_mode_publisher_ =
                    this->create_publisher<px4_msgs::msg::OffboardControlMode>("/px4_1/fmu/in/offboard_control_mode", qos_pub);
            this->trajectory_setpoint_publisher_ =
                    this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/px4_1/fmu/in/trajectory_setpoint", qos_pub);
            this->vehicle_command_publisher_ = 
                    this->create_publisher<px4_msgs::msg::VehicleCommand>("/px4_1/fmu/in/vehicle_command", qos_pub);

I hope this helps.
TB

2 Likes
5

Thank you so much, that is exactly what I needed I greatly appreciate your help. I was able to get the topics for all PX4 instances loaded in Gazebo by running ros2 topic list, and by running ros2 topic echo /px4_3/fmu/in/trajectory_setpoint I can see that the trajectory setpoints that I programmed are being published to the topic of the 3rd PX4-instance but the respective iris drone (i.e., the 3rd PX4-instance) does not move to the desired location (it does not move at all). However when I run make px4_sitl_default gazebo-classic_iris__empty to load only one PX4 iris model in gazebo it works perfectly (i.e., the drone moves to the desired location). Below is the code of the C++ executable (the part of the code that addresses the issue mentioned above).

#include <px4_msgs/msg/offboard_control_mode.hpp>

#include <px4_msgs/msg/trajectory_setpoint.hpp>

#include <px4_msgs/msg/vehicle_command.hpp>

#include <px4_msgs/msg/vehicle_control_mode.hpp>

#include <rclcpp/rclcpp.hpp>

#include <rclcpp/qos.hpp>

#include <stdint.h>

#include

#include

using namespace std::chrono;

using namespace std::chrono_literals;

using namespace px4_msgs::msg;

class OffboardControl : public rclcpp::Node

{

public:

OffboardControl() : Node(“offboard_control”)

{

auto qos_pub = rclcpp::QoS(rclcpp::KeepLast(10));

offboard_control_mode_publisher_ = this->create_publisher(“/px4_3/fmu/in/offboard_control_mode”, qos_pub);

trajectory_setpoint_publisher_ = this->create_publisher(“/px4_3/fmu/in/trajectory_setpoint”, qos_pub);

vehicle_command_publisher_ = this->create_publisher(“/px4_3/fmu/in/vehicle_command”, qos_pub);

functions for publishing the trajectory set points and vehicle commands

void OffboardControl::publish_trajectory_setpoint()

{

TrajectorySetpoint msg{};

msg.position = {-9.0, -8.0, -2.0};

msg.yaw = 3.14; // [-PI:PI]

msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;

trajectory_setpoint_publisher_->publish(msg);

}

void OffboardControl::publish_vehicle_command(uint16_t command, float param1, float param2)

{

VehicleCommand msg{};

msg.param1 = param1;

msg.param2 = param2;

msg.command = command;

msg.target_system = 1;

msg.target_component = 1;

msg.source_system = 1;

msg.source_component = 1;

msg.from_external = true;

msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;

vehicle_command_publisher_->publish(msg);

}

I am guessing that I probably need to do something to set up the QoS settings and that could be why the respective iris drone (i.e., the 3rd PX4-instance) is not moving even when the trajectory setpoints are being published to its topic but I am not sure. Any help would be appreciated in case you know what might be causing this behavior. Thanks again!

6

Hi @Terence-I , I’m glad that you reached this point.
As you saw, in simulation every vehicle receives a unique namespace and that is how you control it individually.

Now, your problem is not related to the QoS (additional details on QoS profiles are here ROS 2 User Guide | PX4 User Guide (main), note that only subscribers needs to adjust their QoS); If you had looked all the way down the links :stuck_out_tongue_winking_eye:

you would have noticed a section called Adjusting the target_system value. Read it: you need to adjust the target_system field in your vehicle_command messages in order to them to be accepted.
Right now the drone is receiving the setpoints but it is not accepting the arm and change to offboard mode commands. You can actually try to change to offboard and arm in QGC, you will see that it works as long as you send trajectory setpoints and OffboardControlMode messages

2 Likes
7

This was very helpful, thank you so much for your help I really appreciate it. :pray:

8

I have one more question about publishing position information from another topic to the px4 iris models’ /fmu/in/trajectory_setpoint topic. In my simulation, I have a ground vehicle publishing its position information to a topic called /robot/odom_robot, and I am trying to publish the position of the ground vehicle from this topic to a /fmu/in/trajectory_setpoint topic of a px4 iris model drone, so that it can fly to the ground vehicle’s position. In the script of the python node I created to do this, I created a subscriber to the /robot/odom_robot topic to get the position information of the ground vehicle, and then I published it to the /fmu/in/trajectory_setpoint topic (below is the lines of code to do this)

  • creating the subscriber

self.subscription = self.create_subscription(Odometry, ‘/robot/odom_robot’, self.odometry_callback, 10)

  • creating the variable to store this position information

self.current_pose = None

  • The subscriber’s callback function

def odometry_callback(self, msg):

      self.current_pose = msg
  • The function to publish the position information to the /fmu/in/trajectory_setpoint topic

def publish_trajectory_setpoint(self):

    msg = TrajectorySetpoint()

    msg.position = [self.current_pose.pose.pose.position.x, self.current_pose.pose.pose.position.y, 2.0]

    msg.yaw = -3.14 # [-PI:PI]

    msg.timestamp = int(Clock().now().nanoseconds / 1000) # time in microseconds

    self.trajectory_setpoint_publisher_.publish(msg)

When I run the node, I can see that the ground vehicle’s position is being published to the /fmu/in/trajectory_setpoint topic by using ros2 topic echo but the iris drone is not flying to the ground vehicle location. I am thinking that this has something to do with QoS settings, and I am not really sure how to access them and/or change them. Any help on this would be highly appreciated. Thank you!

9

Could you confirm that the drone correctly enters offboard mode and it is able to arm and fly, even if it does not go to the desired location?

Please, review the compatibility issues: ROS 2 User Guide | PX4 User Guide pay attention to the frame conventions:
right now you are telling your drone to go 2 meters under the ground (z must be negative).