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;
}
