Mavsdk + px4 control vtol FW velocity

hello, in my project my problem is that i am unable to control my vtol’s velocity by using set_velocity_body() can someone help me out ?

void detectionCallback(const std_msgs::Int32MultiArray::ConstPtr& msg,
                       ros::NodeHandle nh)
{
    message_received = true;
    steady_clock::time_point now = steady_clock::now();

    if (msg->data.empty())
    {
        ROS_INFO("No UAV detected.");
        last_detection_status = false;
        // Optionally, compute and log elapsed time since last detection
        auto elapsed = duration_cast<seconds>(now - last_detection_time).count();
        
    }
    else
    {
        last_detection_status = true;
        // Update last_detection_time every time a valid detection is received
        last_detection_time = now;
        
        // Process detection data
        x = msg->data[0];
        y = msg->data[1];
        w = msg->data[2];
        h = msg->data[3];
        ROS_INFO("UAV detected at x: %d, y: %d, w: %d, h: %d", x, y, w, h);
        log_message("UAV detected at x: " + std::to_string(x) + 
                    ", y: " + std::to_string(y) + 
                    ", w: " + std::to_string(w) + 
                    ", h: " + std::to_string(h));

        if (!in_offboard_mode && g_offboard == nullptr)
        {
            
            g_offboard = std::make_unique<Offboard>(g_system);
            Offboard::VelocityBodyYawspeed stay{};
            g_offboard->set_velocity_body(stay);
            std::cout << "Going to offboard mode...\n";
            Offboard::Result offboard_result = g_offboard->start();
            if (offboard_result == Offboard::Result::Success)
            {
                ROS_INFO("Switched to Offboard Mode.");
                in_offboard_mode = true;
            }
            else
            {
                std::cout << "Offboardmode  upload failed" << std::endl;
            }
        }

        // PID response
        std::vector<double> PIDoutputs = getPidResponse(x, y, w + h, nh);



        g_offboard->set_velocity_body({7.0f,static_cast<float>(-PIDoutputs[0]),static_cast<float>(-PIDoutputs[1]),0.0f});
    }
}

In general, you might as well use MAVROS instead of MAVSDK if you’re already using ROS.

And for next time, when you say “unable” what do you mean? What’s the error? And what versions of PX4, Mavsdk, etc. are you using?

Now that I look at it, I see an issue:

You’re doing a lot of work in the callback. Generally, you’re not supposed to do a lot of work in library callbacks, in order not to block anything. You’re especially not supposed to do IO in it, which you are essentially doing by sending Mavsdk offboard commands.

Now the actual issue happens because you instantiate the offboard plugin inside of the callback, and so when you exit the callback, the plugin goes out of scope and therefore gets destructed. This means offboard control stops sending messages and PX4 will stop executing it, if it has even started by then.

What you need to do instead is set some sort of flag in the callback which then means you will do the Mavsdk work in the main thread, or some thread that stays alive.

thanks for quick reply. alright you are right about that point am gonna work on that and let you know. one more thing is that I am actually doing what you said in the main thread in the code below its a different code I tried and it want not able to make the velocity changes the vtol continued the loiter no turns where made.

//
// Example showing how to make a VTOL takeoff, transition to fixedwing, loiter
// for a bit, and then transition back and return to launch.
//

#include <chrono>
#include <cstdint>
#include <iostream>
#include <thread>
#include <cmath>
#include <future>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/offboard/offboard.h>

using std::this_thread::sleep_for;
using std::chrono::seconds;
using namespace mavsdk;
double position_m;

void usage(const std::string& bin_name)
{
    std::cerr << "Usage : " << bin_name << " <connection_url>\n"
              << "Connection URL format should be :\n"
              << " For TCP : tcp://[server_host][:server_port]\n"
              << " For UDP : udp://[bind_host][:bind_port]\n"
              << " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
              << "For example, to connect to the simulator use URL: udp://:14540\n";
}

std::shared_ptr<System> get_system(Mavsdk& mavsdk)
{
    std::cout << "Waiting to discover system...\n";
    auto prom = std::promise<std::shared_ptr<System>>{};
    auto fut = prom.get_future();

    // We wait for new systems to be discovered, once we find one that has an
    // autopilot, we decide to use it.
    mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
        auto system = mavsdk.systems().back();

        if (system->has_autopilot()) {
            std::cout << "Discovered autopilot\n";

            // Unsubscribe again as we only want to find one system.
            mavsdk.subscribe_on_new_system(nullptr);
            prom.set_value(system);
        }
    });

    // We usually receive heartbeats at 1Hz, therefore we should find a
    // system after around 3 seconds max, surely.
    if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
        std::cerr << "No autopilot found.\n";
        return {};
    }

    // Get discovered system now.
    return fut.get();
}

int main(int argc, char** argv)
{
    if (argc != 2) {
        usage(argv[0]);
        return 1;
    }

    Mavsdk mavsdk;
    ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);

    if (connection_result != ConnectionResult::Success) {
        std::cerr << "Connection failed: " << connection_result << '\n';
        return 1;
    }

    auto system = get_system(mavsdk);
    if (!system) {
        return 1;
    }

    // Instantiate plugins.
    auto telemetry = Telemetry{system};
    auto action = Action{system};

    // We want to listen to the altitude of the drone at 1 Hz.
    const Telemetry::Result set_rate_result = telemetry.set_rate_position(1.0);
    if (set_rate_result != Telemetry::Result::Success) {
        std::cerr << "Setting rate failed: " << set_rate_result << '\n';
        return 1;
    }

    // Set up callback to monitor altitude.
    telemetry.subscribe_position([](Telemetry::Position position) {
        std::cout << "Altitude: " << position.relative_altitude_m << " m\n";
        position_m = position.relative_altitude_m;
    });

    // Wait until we are ready to arm.
    while (!telemetry.health_all_ok()) {
        std::cout << "Waiting for vehicle to be ready to arm...\n";
        sleep_for(seconds(1));
    }

    // Arm vehicle
    std::cout << "Arming.\n";
    const Action::Result arm_result = action.arm();

    if (arm_result != Action::Result::Success) {
        std::cerr << "Arming failed: " << arm_result << '\n';
        return 1;
    }

    // Take off


    float desired_altitude = 20.0f;
    action.set_takeoff_altitude(desired_altitude);
    
    std::cout << "Taking off.\n";
    const Action::Result takeoff_result = action.takeoff();
    if (takeoff_result != Action::Result::Success) {
        std::cerr << "Takeoff failed:n" << takeoff_result << '\n';
        return 1;
    }

    while (true)
    {
        // Wait while it takes off.
        if (position_m >= desired_altitude * 0.95){
            std::cout << "Transition to fixedwing.\n";
            sleep_for(seconds(1));

            const Action::Result fw_result = action.transition_to_fixedwing();
            sleep_for(seconds(1));

            if (fw_result != Action::Result::Success) {
                std::cerr << "Transition to fixed wing failed: " << fw_result << '\n';
                return 1;
            }
            else if (fw_result == Action::Result::Success)
            {
                break;
            }
            
        }else {
            sleep_for(seconds(1));
        }
    }
    

    // Let it transition and start loitering.
    sleep_for(seconds(10));

    auto offboard = Offboard{system};
    Offboard::VelocityBodyYawspeed stay{};
    offboard.set_velocity_body(stay);
    // Offboard modunu başlatmadan önce, offboard kontrolünün başarılı olacağından emin olun
    Offboard::Result offboard_result = offboard.start();
    if (offboard_result != Offboard::Result::Success) {
        std::cerr << "Offboard modu baslatilamadi: " << offboard_result << std::endl;
        return 1;
    }

    std::cout << "Offboard modu baslatildi. Saga donud yapiliyor..." << std::endl;

    // Sağa dönüş için y hızını pozitif yapın
    Offboard::VelocityBodyYawspeed saga_donus{0.0f, 5.0f, 0.0f, 0.0f};

    offboard.set_velocity_body(saga_donus);
    sleep_for(seconds(5)); // 5 saniye boyunca sağa dönüş yap

    std::cout << "Şimdi sola dönüş yapılıyor..." << std::endl;

    // Sola dönüş için y hızını negatif yapın
    Offboard::VelocityBodyYawspeed sola_donus{0.0f, -5.0f, 0.0f, 0.0f};

    offboard.set_velocity_body(sola_donus);
    sleep_for(seconds(5)); // 5 saniye boyunca sola dönüş yap

    // Yukarı hareket için down_m_s negatif yapılır
    Offboard::VelocityBodyYawspeed yukari_hareket{0.0f, 0.0f, 2.0f, 0.0f};
    std::cout << "Yukarı hareket ediliyor..." << std::endl;

    offboard.set_velocity_body(yukari_hareket);
    sleep_for(seconds(5)); // 5 saniye boyunca yukarı hareket et

    // Aşağı hareket için down_m_s pozitif yapılır
    Offboard::VelocityBodyYawspeed asagi_hareket{0.0f, 0.0f, -2.0f, 0.0f};
    std::cout << "Aşağı hareket ediliyor..." << std::endl;

    offboard.set_velocity_body(asagi_hareket);
    sleep_for(seconds(5)); // 5 saniye boyunca aşağı hareket et


    // Offboard modunu durdur
    offboard_result = offboard.stop();
    if (offboard_result != Offboard::Result::Success) {
        std::cerr << "Offboard modu durdurulamadı: " << offboard_result << std::endl;
        return 1;
    }

    std::cout << "Offboard modu durduruldu." << std::endl;
    std::cout << "Burada1 \n";

    // Let's stop before reaching the goto point and go back to hover.
    std::cout << "Transition back to multicopter...\n";
    const Action::Result mc_result = action.transition_to_multicopter();
    sleep_for(seconds(1));
    std::cout << "Burada2 \n";

    if (mc_result != Action::Result::Success) {
        std::cerr << "Transition to multi copter failed: " << mc_result << '\n';
        return 1;
    }

    // Let's stop before reaching the goto point and go back to hover.
    std::cout << "Transition back to multicopter...\n";
    const Action::Result mc_result2 = action.transition_to_multicopter();
    sleep_for(seconds(1));

    if (mc_result != Action::Result::Success) {
        std::cerr << "Transition to multi copter failed: " << mc_result2 << '\n';
        return 1;
    }

    // Wait for the transition to be carried out.
    sleep_for(seconds(5));

    // Now just land here.
    std::cout << "Landing...\n";
    const Action::Result land_result = action.land();
    if (land_result != Action::Result::Success) {
        std::cerr << "Land failed: " << land_result << '\n';
        return 1;
    }

    // Wait until disarmed.
    while (telemetry.armed()) {
        std::cout << "Waiting for vehicle to land and disarm\n.";
        sleep_for(seconds(1));
    }

    std::cout << "Disarmed, exiting.\n";

    return 0;
}

px4 version: 1.16.0alpha
mavsdk version: 1.4.17
ros noetic
also I have tried using mavros and I was unable to give the velocity

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandVtolTransition.h>
#include <mavros_msgs/Altitude.h>
#include <thread>
#include <chrono>

using std::chrono::seconds;
using std::this_thread::sleep_for;

mavros_msgs::State current_state;
double current_altitude = 0.0;
double desired_altitude = 10.0;

void state_cb(const mavros_msgs::State::ConstPtr& msg) {
    current_state = *msg;
}

void altitude_cb(const mavros_msgs::Altitude::ConstPtr& msg) {
    current_altitude = msg->relative;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "mavros_control");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    ros::ServiceClient transition_client = nh.serviceClient<mavros_msgs::CommandVtolTransition>("mavros/cmd/vtol_transition");
    ros::Subscriber alt_sub = nh.subscribe<mavros_msgs::Altitude>("mavros/altitude", 10, altitude_cb);
    ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>("mavros/setpoint_velocity/cmd_vel", 10);

    ros::Rate rate(20.0);

    // Wait for FCU connection
    while (ros::ok() && !current_state.connected) {
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = desired_altitude;

    // Send setpoints for initialization
    for (int i = 100; ros::ok() && i > 0; --i) {
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    if (!current_state.armed) {
        if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
            ROS_INFO("Vehicle armed");
        } else {
            ROS_ERROR("Arming failed");
            return -1;
        }
    }

    if (current_state.mode != "OFFBOARD") {
        if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
            ROS_INFO("Offboard enabled");
        }
    }

    // Wait until reaching desired altitude
    while (ros::ok() && current_altitude < desired_altitude * 0.80) {
        local_pos_pub.publish(pose);
        ros::spinOnce();
        ROS_INFO_THROTTLE(2, "Current Altitude: %.2f m", current_altitude);
        rate.sleep();
    }

    // VTOL Transition to Fixed Wing
    mavros_msgs::CommandVtolTransition vtol_cmd;
    vtol_cmd.request.header.stamp = ros::Time::now();
    vtol_cmd.request.state = mavros_msgs::CommandVtolTransition::Request::STATE_FW;

    if (transition_client.call(vtol_cmd) && vtol_cmd.response.success) {
        ROS_INFO("VTOL transition to FW command sent");
    } else {
        ROS_WARN("Failed to send VTOL transition command");
    }

    sleep_for(seconds(10));

   
    geometry_msgs::TwistStamped vel_cmd;
    vel_cmd.header.frame_id = "base_link";  

    
    const float FORWARD_SPEED = 15.0;  
    const float LATERAL_SPEED = 2.0;

    
    while (ros::ok()) {
        
        vel_cmd.twist.linear.x = FORWARD_SPEED;
        vel_cmd.twist.linear.y = LATERAL_SPEED;  
       
        vel_cmd.header.stamp = ros::Time::now();
        vel_pub.publish(vel_cmd);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

@JulianOes could you help me out with this issue ?