Mavros change yaw

asked 2017-11-07 08:33:31 -0600

MaximeRector gravatar image

updated 2017-11-07 09:19:33 -0600

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

edit retag flag offensive close merge delete

Comments

In your second edit you say

Problem solved

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

jayess gravatar imagejayess ( 2017-11-14 00:15:47 -0600 )edit