Why does my quadrotor becomes unstable after turning?

edit

Hello everyone, hope you are all having a great time.
I'm trying to write a simple test mavros program 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 is that I noticed, 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, it shakes badly! and also the last turn before landing, seems to not to take place properly as well!
What am I missing here? any help is greatly appreciated.
Here are two video clips I recorded 1 shows the quadrotor with my loiter_fn, 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 ...