Simulate multiple vehicles based on RTPS/DDS in Gazebo

[mengchaoheng]

Describe the bug

I want to run ros node in off board mode to control multiple uavs, like https://docs.px4.io/main/en/simulation/multi_vehicle_simulation_gazebo.html. Except for the first uav, other uavs do not respond.

To Reproduce

Steps to reproduce the behavior:
1.Clone the PX4/Firmware code， checkout to 30150f7, then build the SITL code and run DONT_RUN=1 make px4_sitl_rtps gazebo
2.Build the micrortps_agent. To use the agent in ROS 2, follow the instructions here, and the px4_ros2_com checkout to 7e25c34, px4_msg checkout to daee121, and then I have been run python3 uorb_to_ros_msgs.py ~/PX4-Autopilot/msg/ ~/px4_ros_com_ros2/src/px4_msgs/msg/ to update msg.
3. Run ./Tools/simulation/gazebo/sitl_multiple_run.sh. For example, to spawn 2 vehicles, run:
./Tools/simulation/gazebo/sitl_multiple_run.sh -t px4_sitl_rtps -m iris -n 2
4.Run micrortps_agent. For example, to connect 2 vehicles, run:
micrortps_agent -t UDP -r 2020 -s 2019 -n vhcl0 and micrortps_agent -t UDP -r 2022 -s 2021 -n vhcl1 on two terminal.
5. Run the ros node in offboard mode to control multiple uavs. Like:
ros2 run px4_ros_com offboard_control_vhcl0
and
ros2 run px4_ros_com offboard_control_vhcl1
7. see error...

Expected behavior

After step 5, all uav take off.

The offboard node code.

offboard_control_vhcl0.cpp and offboard_control_vhcl1.cpp is modified from offboard_control.cpp

offboard_control_vhcl0.cpp：

#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/timesync.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_control_mode.hpp>
#include <rclcpp/rclcpp.hpp>
#include <stdint.h>

#include <chrono>
#include <iostream>

using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;

class OffboardControl : public rclcpp::Node {
public:
	OffboardControl() : Node("offboard_control_vhcl0") {
#ifdef ROS_DEFAULT_API
		offboard_control_mode_publisher_ =
			this->create_publisher<OffboardControlMode>("vhcl0/fmu/offboard_control_mode/in", 10);
		trajectory_setpoint_publisher_ =
			this->create_publisher<TrajectorySetpoint>("vhcl0/fmu/trajectory_setpoint/in", 10);
		vehicle_command_publisher_ =
			this->create_publisher<VehicleCommand>("vhcl0/fmu/vehicle_command/in", 10);
#else
		offboard_control_mode_publisher_ =
			this->create_publisher<OffboardControlMode>("vhcl0/fmu/offboard_control_mode/in");
		trajectory_setpoint_publisher_ =
			this->create_publisher<TrajectorySetpoint>("vhcl0/fmu/trajectory_setpoint/in");
		vehicle_command_publisher_ =
			this->create_publisher<VehicleCommand>("vhcl0/fmu/vehicle_command/in");
#endif

		// get common timestamp
		timesync_sub_ =
			this->create_subscription<px4_msgs::msg::Timesync>("vhcl0/fmu/timesync/out", 10,
				[this](const px4_msgs::msg::Timesync::UniquePtr msg) {
					timestamp_.store(msg->timestamp);
				});

		offboard_setpoint_counter_ = 0;

		auto timer_callback = [this]() -> void {

			if (offboard_setpoint_counter_ == 10) {
				// Change to Offboard mode after 10 setpoints
				this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);

				// Arm the vehicle
				this->arm();
			}

            		// offboard_control_mode needs to be paired with trajectory_setpoint
			publish_offboard_control_mode();
			publish_trajectory_setpoint();
				std::cout << "offboard_setpoint_counter_: " << offboard_setpoint_counter_<< std::endl;

           		 // stop the counter after reaching 11
			//if (offboard_setpoint_counter_ < 11) {
				offboard_setpoint_counter_++;
			//}
		};
		timer_ = this->create_wall_timer(100ms, timer_callback);
	}

	void arm() const;
	void disarm() const;

private:
	rclcpp::TimerBase::SharedPtr timer_;

	rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
	rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
	rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_;
	rclcpp::Subscription<px4_msgs::msg::Timesync>::SharedPtr timesync_sub_;

	std::atomic<uint64_t> timestamp_;   //!< common synced timestamped

	uint64_t offboard_setpoint_counter_;   //!< counter for the number of setpoints sent

	void publish_offboard_control_mode() const;
	void publish_trajectory_setpoint() const;
	void publish_vehicle_command(uint16_t command, float param1 = 0.0,
				     float param2 = 0.0) const;
};

/**
 * @brief Send a command to Arm the vehicle
 */
void OffboardControl::arm() const {
	publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);

	RCLCPP_INFO(this->get_logger(), "Arm command send");
}

/**
 * @brief Send a command to Disarm the vehicle
 */
void OffboardControl::disarm() const {
	publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);

	RCLCPP_INFO(this->get_logger(), "Disarm command send");
}

/**
 * @brief Publish the offboard control mode.
 *        For this example, only position and altitude controls are active.
 */
void OffboardControl::publish_offboard_control_mode() const {
	OffboardControlMode msg{};
	msg.timestamp = timestamp_.load();
	msg.position = true;
	msg.velocity = false;
	msg.acceleration = false;
	msg.attitude = false;
	msg.body_rate = false;

	offboard_control_mode_publisher_->publish(msg);
}


/**
 * @brief Publish a trajectory setpoint
 *        For this example, it sends a trajectory setpoint to make the
 *        vehicle hover at 5 meters with a yaw angle of 180 degrees.
 */
void OffboardControl::publish_trajectory_setpoint() const {
	TrajectorySetpoint msg{};
	msg.timestamp = timestamp_.load();
	float T=1.0f;
	float _length = 5.f;
	float t=offboard_setpoint_counter_/100.0f;
	float x = _length*sin(2.f*3.14f*t/T);
	float y = _length*sin(2.f* 2.f*3.14f*t/T);
	msg.position = {x, y, -5.0};
	//if (offboard_setpoint_counter_<=50){
	//	msg.position = {0.0, offboard_setpoint_counter_/100.0, -5.0};
	//}else if (50<offboard_setpoint_counter_<=100){
	//	msg.position = {0.0, 10.0, -5.0};
	//}else if (100<offboard_setpoint_counter_<=150){
	//	msg.position = {5.0, 5.0, -5.0};
	//}else if (150<offboard_setpoint_counter_<=200){
	//	msg.position = {10.0, 5.0, -5.0};
	//}else{
	//	msg.position = {0.0, 0.0, -2.0};
	//}
	msg.yaw = -3.14; // [-PI:PI]

	trajectory_setpoint_publisher_->publish(msg);
}

/**
 * @brief Publish vehicle commands
 * @param command   Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
 * @param param1    Command parameter 1
 * @param param2    Command parameter 2
 */
void OffboardControl::publish_vehicle_command(uint16_t command, float param1,
					      float param2) const {
	VehicleCommand msg{};
	msg.timestamp = timestamp_.load();
	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;

	vehicle_command_publisher_->publish(msg);
}

int main(int argc, char* argv[]) {
	std::cout << "Starting offboard control node..." << std::endl;
	setvbuf(stdout, NULL, _IONBF, BUFSIZ);
	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<OffboardControl>());

	rclcpp::shutdown();
	return 0;
}

offboard_control_vhcl1.cpp:

#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/timesync.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_control_mode.hpp>
#include <rclcpp/rclcpp.hpp>
#include <stdint.h>

#include <chrono>
#include <iostream>

using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;

class OffboardControl : public rclcpp::Node {
public:
	OffboardControl() : Node("offboard_control_vhcl1") {
#ifdef ROS_DEFAULT_API
		offboard_control_mode_publisher_ =
			this->create_publisher<OffboardControlMode>("vhcl1/fmu/offboard_control_mode/in", 10);
		trajectory_setpoint_publisher_ =
			this->create_publisher<TrajectorySetpoint>("vhcl1/fmu/trajectory_setpoint/in", 10);
		vehicle_command_publisher_ =
			this->create_publisher<VehicleCommand>("vhcl1/fmu/vehicle_command/in", 10);
#else
		offboard_control_mode_publisher_ =
			this->create_publisher<OffboardControlMode>("vhcl1/fmu/offboard_control_mode/in");
		trajectory_setpoint_publisher_ =
			this->create_publisher<TrajectorySetpoint>("vhcl1/fmu/trajectory_setpoint/in");
		vehicle_command_publisher_ =
			this->create_publisher<VehicleCommand>("vhcl1/fmu/vehicle_command/in");
#endif

		// get common timestamp
		timesync_sub_ =
			this->create_subscription<px4_msgs::msg::Timesync>("vhcl1/fmu/timesync/out", 10,
				[this](const px4_msgs::msg::Timesync::UniquePtr msg) {
					timestamp_.store(msg->timestamp);
				});

		offboard_setpoint_counter_ = 0;

		auto timer_callback = [this]() -> void {

			if (offboard_setpoint_counter_ == 10) {
				// Change to Offboard mode after 10 setpoints
				this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);

				// Arm the vehicle
				this->arm();
			}

            		// offboard_control_mode needs to be paired with trajectory_setpoint
			publish_offboard_control_mode();
			publish_trajectory_setpoint();
				std::cout << "offboard_setpoint_counter_: " << offboard_setpoint_counter_<< std::endl;

           		 // stop the counter after reaching 11
			//if (offboard_setpoint_counter_ < 11) {
				offboard_setpoint_counter_++;
			//}
		};
		timer_ = this->create_wall_timer(100ms, timer_callback);
	}

	void arm() const;
	void disarm() const;

private:
	rclcpp::TimerBase::SharedPtr timer_;

	rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
	rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
	rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_;
	rclcpp::Subscription<px4_msgs::msg::Timesync>::SharedPtr timesync_sub_;

	std::atomic<uint64_t> timestamp_;   //!< common synced timestamped

	uint64_t offboard_setpoint_counter_;   //!< counter for the number of setpoints sent

	void publish_offboard_control_mode() const;
	void publish_trajectory_setpoint() const;
	void publish_vehicle_command(uint16_t command, float param1 = 0.0,
				     float param2 = 0.0) const;
};

/**
 * @brief Send a command to Arm the vehicle
 */
void OffboardControl::arm() const {
	publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);

	RCLCPP_INFO(this->get_logger(), "Arm command send");
}

/**
 * @brief Send a command to Disarm the vehicle
 */
void OffboardControl::disarm() const {
	publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);

	RCLCPP_INFO(this->get_logger(), "Disarm command send");
}

/**
 * @brief Publish the offboard control mode.
 *        For this example, only position and altitude controls are active.
 */
void OffboardControl::publish_offboard_control_mode() const {
	OffboardControlMode msg{};
	msg.timestamp = timestamp_.load();
	msg.position = true;
	msg.velocity = false;
	msg.acceleration = false;
	msg.attitude = false;
	msg.body_rate = false;

	offboard_control_mode_publisher_->publish(msg);
}


/**
 * @brief Publish a trajectory setpoint
 *        For this example, it sends a trajectory setpoint to make the
 *        vehicle hover at 5 meters with a yaw angle of 180 degrees.
 */
void OffboardControl::publish_trajectory_setpoint() const {
	TrajectorySetpoint msg{};
	msg.timestamp = timestamp_.load();
	float T=1.0f;
	float _length = 5.f;
	float t=offboard_setpoint_counter_/100.0f;
	float x = _length*sin(2.f*3.14f*t/T);
	float y = _length*sin(2.f* 2.f*3.14f*t/T);
	msg.position = {x, y, -10.0};
	//if (offboard_setpoint_counter_<=50){
	//	msg.position = {0.0, offboard_setpoint_counter_/100.0, -5.0};
	//}else if (50<offboard_setpoint_counter_<=100){
	//	msg.position = {0.0, 10.0, -5.0};
	//}else if (100<offboard_setpoint_counter_<=150){
	//	msg.position = {5.0, 5.0, -5.0};
	//}else if (150<offboard_setpoint_counter_<=200){
	//	msg.position = {10.0, 5.0, -5.0};
	//}else{
	//	msg.position = {0.0, 0.0, -2.0};
	//}
	msg.yaw = -3.14; // [-PI:PI]

	trajectory_setpoint_publisher_->publish(msg);
}

/**
 * @brief Publish vehicle commands
 * @param command   Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
 * @param param1    Command parameter 1
 * @param param2    Command parameter 2
 */
void OffboardControl::publish_vehicle_command(uint16_t command, float param1,
					      float param2) const {
	VehicleCommand msg{};
	msg.timestamp = timestamp_.load();
	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;

	vehicle_command_publisher_->publish(msg);
}

int main(int argc, char* argv[]) {
	std::cout << "Starting offboard control node..." << std::endl;
	setvbuf(stdout, NULL, _IONBF, BUFSIZ);
	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<OffboardControl>());

	rclcpp::shutdown();
	return 0;
}

[mengchaoheng]

@Jaeyoung-Lim How can I do this? Control each px4 node.

[mengchaoheng]

when I open the QGC and manuly arm the second UAV and then change the fly mode, it can takeoff !!! Can I omit the manual switching process?

[mengchaoheng]

@Jaeyoung-Lim

[mengchaoheng]

mch@ubuntu:~/PX4-Autopilot$ ./Tools/simulation/gazebo/sitl_multiple_run.sh -t px4_sitl_rtps -m iris -n 2

killing running instances
GAZEBO_PLUGIN_PATH :/home/mch/PX4-Autopilot/Tools/simulation/gazebo/../../../build/px4_sitl_rtps/build_gazebo
GAZEBO_MODEL_PATH :/home/mch/PX4-Autopilot/Tools/simulation/gazebo/../../../Tools/simulation/gazebo/sitl_gazebo/models
LD_LIBRARY_PATH /opt/ros/foxy/opt/yaml_cpp_vendor/lib:/opt/ros/foxy/opt/rviz_ogre_vendor/lib:/opt/ros/foxy/lib/x86_64-linux-gnu:/opt/ros/foxy/lib:/home/mch/PX4-Autopilot/Tools/simulation/gazebo/../../../build/px4_sitl_rtps/build_gazebo
Starting gazebo
Gazebo multi-robot simulator, version 11.12.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Err] [Plugin.hh:212] Failed to load plugin libgazebo_ros_init.so: libgazebo_ros_init.so: cannot open shared object file: No such file or directory
[Err] [Plugin.hh:212] Failed to load plugin libgazebo_ros_factory.so: libgazebo_ros_factory.so: cannot open shared object file: No such file or directory
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.101.243
[Msg] Loading world file [/home/mch/PX4-Autopilot/Tools/simulation/gazebo/../../../Tools/simulation/gazebo/sitl_gazebo/worlds/empty.world]
starting instance 0 in /home/mch/PX4-Autopilot/build/px4_sitl_rtps/rootfs/0
/home/mch/PX4-Autopilot/Tools/simulation/gazebo/../../../Tools/simulation/gazebo/sitl_gazebo/models/iris/iris.sdf.jinja -> /tmp/iris_0.sdf
Spawning iris_0 at 0.0 0
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
starting instance 1 in /home/mch/PX4-Autopilot/build/px4_sitl_rtps/rootfs/1
[Wrn] [gazebo_gps_plugin.cpp:77] [gazebo_gps_plugin]: iris_0::gps0 using gps topic "gps0"
[Wrn] [gazebo_gps_plugin.cpp:202] [gazebo_gps_plugin] Using default update rate of 5hz 
[Wrn] [msgs.cc:1843] Conversion of sensor type[magnetometer] not supported.
/home/mch/PX4-Autopilot/Tools/simulation/gazebo/../../../Tools/simulation/gazebo/sitl_gazebo/models/iris/iris.sdf.jinja -> /tmp/iris_1.sdf
Spawning iris_1 at 0.0 3
[Msg] Connecting to PX4 SITL using TCP
[Msg] Lockstep is enabled
[Msg] Speed factor set to: 1
[Wrn] [gazebo_mavlink_interface.cpp:153] No identifier on joint. Using 0 as default sensor ID
[Wrn] [gazebo_mavlink_interface.cpp:153] No identifier on joint. Using 0 as default sensor ID
[Msg] Using MAVLink protocol v2.0
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
Starting gazebo client
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring.
[Wrn] [gazebo_gps_plugin.cpp:77] [gazebo_gps_plugin]: iris_1::gps0 using gps topic "gps0"
[Wrn] [gazebo_gps_plugin.cpp:202] [gazebo_gps_plugin] Using default update rate of 5hz 
[Wrn] [msgs.cc:1843] Conversion of sensor type[magnetometer] not supported.
[Msg] Connecting to PX4 SITL using TCP
[Msg] Lockstep is enabled
[Msg] Speed factor set to: 1
[Wrn] [gazebo_mavlink_interface.cpp:153] No identifier on joint. Using 0 as default sensor ID
[Wrn] [gazebo_mavlink_interface.cpp:153] No identifier on joint. Using 0 as default sensor ID
[Msg] Using MAVLink protocol v2.0
[Wrn] [msgs.cc:1843] Conversion of sensor type[magnetometer] not supported.
[Wrn] [msgs.cc:1843] Conversion of sensor type[magnetometer] not supported.
[Wrn] [msgs.cc:1843] Conversion of sensor type[magnetometer] not supported.
[Wrn] [msgs.cc:1843] Conversion of sensor type[magnetometer] not supported.

[LiuWhale]

I think it may be caused by the reaseon that every UAV needs a RC in default, but simulation just have one in qgc, you can search a param in qgc to make UAV ignore RC, I remember param is COM_RCL_EXCEPT.

[mad0x60]

QGC only considers the manual control (the joystick in your case) for only the active vehicle. Therefore, you need to change COM_RCL_EXCEPT for every vehicle such that it ignores remote control loss when OFFBOARD is engaged.
Also, you need to specify the target system id of each drone.