Robotics StackExchange | Archived questions

Mavros change yaw

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

Asked by MaximeRector on 2017-11-07 09:33:31 UTC

Comments

In your second edit you say

Problem solved

If that's so, then could you please write what you did as an answer?

Asked by jayess on 2017-11-14 01:15:47 UTC

I am having an issue. Can you guys help me out? https://answers.ros.org/question/386816/setpoint_rawlocal-not-working-with-if-condition/

Asked by subarashi on 2021-09-21 05:29:32 UTC

Answers