Mavros change yaw

edit

Hi,

I'm been using mavros in combination with gazebo to simulate a flying drone. I have already been able to change the x,y,z position of the drone, but now i'm wondering if it's possible to change the yaw of the drone. Can anyone help me with this problem?

Thanks!

#include "classheader_dronecontrol.hpp"

DroneControl::DroneControl(ros::NodeHandle n)
{
    initRos(n);
}

void DroneControl::initRos(ros::NodeHandle n)
{
    cout << "Start Initialising" << endl;
    this->state_sub = n.subscribe<mavros_msgs::State>("mavros/state", 10, &DroneControl::stateCallback, this);
    this->local_pos_pub = n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
    this->arming_client = n.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    this->set_mode_client = n.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    this->KFarucomarkerpos_sub = n.subscribe("droneposition", 1, &DroneControl::droneControlCallback, this);
    cout << "Initialising done!" << endl;

    //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();
    }

    this->pose.pose.position.x = 0;
    this->pose.pose.position.y = 0;
    this->pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        this->local_pos_pub.publish(this->pose);
        ros::spinOnce();
        rate.sleep();
    }

    this->offb_set_mode.request.custom_mode = "OFFBOARD";
    this->arm_cmd.request.value = true;
    last_request = ros::Time::now();

    this->set_mode_client.call(this->offb_set_mode);
    this->offb_set_mode.response.mode_sent;
    ROS_INFO("Offboard enabled");
    this->local_pos_pub.publish(this->pose);
    ros::spinOnce();
    this->arming_client.call(this->arm_cmd);
    ROS_INFO("Vehicle armed");
    this->local_pos_pub.publish(this->pose);
    ros::spinOnce();
}

void DroneControl::stateCallback(const mavros_msgs::State::ConstPtr& msg)
{
    this->current_state = *msg;
}

void DroneControl::droneControlCallback(const kalmanfilter_node::KFarucomarkerpos& msg)
{
    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    this->xm = ((float)msg.xm)/1;
    this->ym = ((float)msg.ym)/1;
    this->zm = ((float)msg.zm)/1;
    this->roll = (float)msg.roll;
    this->pitch = (float)msg.pitch;
    this->yaw = (float)msg.yaw;
    cout << "xm = " << this->xm << endl;
    cout << "ym = " << this->ym << endl;
    cout << "zm = " << this->zm << endl;
    cout << "-----" << endl;;

    this->pose.pose.position.x = this->xm;
    this->pose.pose.position.y = this->ym;
    this->pose.pose.position.z = this->zm;

    this->local_pos_pub.publish(this->pose);
    ros::spinOnce();
    rate.sleep();
}

EDIT

I found that with this->pose.pose.orientation = tf::createQuaternionMsgFromYaw(this->yaw); I can change the yaw but whit this the drone will sometimes do some weird rotations, but my yaw angle (in degrees) is steady.

EDIT2

createQuaternionMsgFromYaw accepts angles in radiant , not in degrees. Problem solved