search

PX4 / PX4-Autopilot

PX4 Autopilot Software
https://px4.io
BSD 3-Clause "New" or "Revised" License
8.48k stars 13.5k forks source link

Simulate multiple vehicles based on RTPS/DDS in Gazebo #20469

Open mengchaoheng opened 2 years ago

mengchaoheng commented 2 years ago

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 30150f723a, 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.

  1. 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.
  2. 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
  3. 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 commented 2 years ago

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

mengchaoheng commented 2 years ago

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 commented 2 years ago

@Jaeyoung-Lim

mengchaoheng commented 2 years ago 
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 commented 1 year ago

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 commented 1 year ago

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.