0
$\begingroup$

I'm trying to write a simple test MAVROS node where my quadrotor is expected to go in a straight line and then have 180-degree turn and come back and land on where it initially took off.
The issue that I'm facing now is, if I suddenly change the yaw, it shocks the quadrotor (creates quiet a bit of turbulence) and thus I want to have as smooth of a turn as possible.
So for this I thought, maybe hovering in the last position for a couple of iterations would do the trick. However, I noticed after adding this snippet, the drone flies unstably, shakes badly and the last turn before landing, seems to not to take place properly as well!

What am I missing here?
Here are two video clips I recorded 1 shows the quadrotor with my loiter_fn(hover in place for some iterations before changing the yaw), and the other without it.

Clip1 : https://files.fm/f/4ts4b4kdv
Clip2: https://files.fm/f/687gmhj6d
and here is my code :

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <tf/LinearMath/Quaternion.h>

// https://docs.px4.io/master/en/simulation/ros_interface.html

mavros_msgs::State current_state;

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

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    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");

    // The setpoint publishing rate MUST be faster than 2Hz
    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 = 0;

    auto to_degree = [](const double &radian_angle)
    {
        return radian_angle * (180 / 3.14);
    };
    auto to_radian = [](const double &degree_angle)
    {
        return (degree_angle * 3.14) / 180;
    };

    tf::Quaternion qtr;
    float yaw = to_radian(180.0);
    qtr.setRPY(0, 0, yaw);
    pose.pose.orientation.w = qtr.getW();
    pose.pose.orientation.x = qtr.getX();
    pose.pose.orientation.y = qtr.getY();
    pose.pose.orientation.z = qtr.getZ();

    // Send a few setpoints before starting
    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();

    bool land = false;
    int i = 0;
    int starting_point = 0;
    float angles[] = {180.0, 360.0};

    // Hovers inplace for 'counter' iterations
    auto loiter_fn = [&](const int& wait_counter)
    {
        pose.pose.orientation.x = 1;
        for (size_t i = 0; i < wait_counter; i++)
        {
            local_pos_pub.publish(pose);
        }
    };

        // Changes the yaw
    auto change_yaw_angle = [&](const float &angle_degree, const int& wait_counter=5)
    {
        loiter_fn(wait_counter);
        qtr.setRPY(0, 0, to_radian(angle_degree));
        pose.pose.orientation.w = qtr.getW();
        pose.pose.orientation.x = qtr.getX();
        pose.pose.orientation.y = qtr.getY();
        pose.pose.orientation.z = qtr.getZ();
    };
    
    
    while (ros::ok())
    {
        if (current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))) 
        {
            if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
                std::cout << "Offboard enabled!\n";
                starting_point = i;
            }

            std::cout << "current_state.mode: " << current_state.mode << std::endl;

            last_request = ros::Time::now();
        }
        else
        {
            if (!current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))) //5
            {
                if (arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                    std::cout << "Vehicle armed!\n";
                }
                last_request = ros::Time::now();
            }
        }

        std::cout << "status: " << current_state.mode << " " << i << "\n";
        if (current_state.mode == "OFFBOARD")
        {
            pose.pose.position.x = i;
            pose.pose.position.y = 0;
            auto z = pose.pose.position.z;
            pose.pose.position.z = z < 30 ? 0.5 * i : 30;
            pose.pose.orientation.x = 5;

            if (i > starting_point + 400)
            {
                change_yaw_angle(180);
                // since counter is not a good measure, we have to manually set an 
                // offset that resembles home
                pose.pose.position.x = -(i - (starting_point + 552));
                std::cout << "status(returning): " << current_state.mode << " x: " << pose.pose.position.x << "\n";
                if (i == 800)
                {
                    change_yaw_angle(360);
                    land = true;
                }
            }
        }

        i++;
        // Important Note
        // This line is a must have, before switching to offboard, we must have already sent
        // a queue of positions. here we are already in a loop. so we can publish the previous location
        // what ever it is (it will hover on that specific location) until the pose is updated.
        local_pos_pub.publish(pose);

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

        if (land)
        {
            break;
        }
    }

    return 0;
}
```
$\endgroup$
1
$\begingroup$

Use setpoint_velocity to turn. When you use position_setpoints then the drone will try to accelerate then decelerate causing this disturbance. By sending cmd_vel setpoints you can have a smooth turn and then you can publish your return setpoint.

Topic name. /mavros/setpoint_velocity/cmd_vel Message docs.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.