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
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