I want to control Yaw in offboard mode
it is my offboard mode code
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <stdio.h>
#include “geometry_msgs/PoseStamped.h”
#include “geometry_msgs/Vector3Stamped.h”
#include “iostream”
#include “fstream”
#include “string”
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
using namespace std;
#define BAUDRATE B9600 //보드레이트를 지정합니다
#define MODEMDEVICE “/dev/rfcomm0” //통신할 직렬포트를 지정합니다
#define _POSIX_SOURCE 1
#define FALSE 0
#define TRUE 1
volatile int STOP=FALSE;
int main(int argc, char **argv)
{
ros::init(argc, argv, “pub_setpoints”);
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local",100);
ros::Rate loop_rate(100);
ros::spinOnce();
geometry_msgs::PoseStamped msg;
int count = 1;
//PositionReciever qp;:
//Body some_object;
//qp.connect_to_server();
int fd, i, c;
int res;
struct termios oldtio, newtio;
char buf[1024];
fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY);
if (fd <0) {
perror(MODEMDEVICE); exit(-1);
}
tcgetattr(fd,&oldtio);
bzero(&newtio, sizeof(newtio));
newtio.c_cflag = BAUDRATE | CS8 | CLOCAL | CREAD;
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
newtio.c_lflag = ICANON;
newtio.c_cc[VTIME] = 0;
newtio.c_cc[VMIN] = 1;
tcflush(fd, TCIFLUSH);
tcsetattr(fd,TCSANOW,&newtio);
char q,w,e;
char *end;
double xx,yy,zz;
while(ros::ok()){
//some_object = qp.getStatus();
// some_object.print();
//printf("%f\n",some_object.position_x);
/*double d;
ifstream myfile("/home/woo/catkin_ws/src/modudculab_ros/src/data.txt");
myfile >> d;
printf(“position z = %f\n”,d);
myfile.close();
*/
res = read(fd, buf, 1024);
buf[res] = 0;
if (buf[0]==‘z’) STOP=TRUE;
xx = strtof(buf, &end);
yy = strtof(end, &end);
zz = strtof(end, NULL);
printf("position = %f, %f, %f\n", xx,yy,zz); // buf의 데이터가 출력, res는 데이터의 길이입니다.
msg.header.stamp = ros::Time::now();
msg.header.seq=count;
msg.header.frame_id = 1;
msg.pose.position.x = xx;//0.001*some_object.position_x;
msg.pose.position.y = yy;//0.001*some_object.position_y;
msg.pose.position.z = zz;//0.001*some_object.position_z;
msg.pose.orientation.x = 0;
msg.pose.orientation.y = 0;
msg.pose.orientation.z = 0;
msg.pose.orientation.w = 1;
chatter_pub.publish(msg);
ros::spinOnce();
count++;
loop_rate.sleep();
}
tcsetattr(fd,TCSANOW,&oldtio);
}
what code should I add to control yaw???/
sorry, I am not good at English
help me please I love you guys