I want to control Yaw in offboard mode

#1

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

#2

Use setpoint_raw/local and set position and yaw

#3
msg.position.x = 0.0;//0.001*some_object.position_x;
        ^

/home/woo/catkin_ws/src/modudculab_ros/src/pub_setpoints_pos.cpp:32:12: error: ‘geometry_msgs::PoseStamped {aka struct geometry_msgs::PoseStamped_<std::allocator >}’ has no member named ‘position’
msg.position.y = 0.0;//0.001some_object.position_y;
^
/home/woo/catkin_ws/src/modudculab_ros/src/pub_setpoints_pos.cpp:33:12: error: ‘geometry_msgs::PoseStamped {aka struct geometry_msgs::PoseStamped_<std::allocator >}’ has no member named ‘position’
msg.position.z = 1.0;//0.001
some_object.position_z;
^
/home/woo/catkin_ws/src/modudculab_ros/src/pub_setpoints_pos.cpp:34:12: error: ‘geometry_msgs::PoseStamped {aka struct geometry_msgs::PoseStamped_<std::allocator >}’ has no member named ‘yaw’
msg.yaw = 0;
^
This is catkin build error

#4

I did change /mavros/setpoint_position/local to /mavros/setpoint_raw/target_local.

but there some errors in catkin build…

how can i fix it??

fixed code below

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <stdio.h>
#include “geometry_msgs/PoseStamped.h”
#include “geometry_msgs/Vector3Stamped.h”

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

while(ros::ok()){
//some_object = qp.getStatus();
// some_object.print();
//printf("%f\n",some_object.position_x);
msg.header.stamp = ros::Time::now();
msg.header.seq=count;
msg.header.frame_id = 1;
msg.position.x = 0.0;//0.001some_object.position_x;
msg.position.y = 0.0;//0.001
some_object.position_y;
msg.position.z = 1.0;//0.001*some_object.position_z;
msg.yaw = 0;

   chatter_pub.publish(msg);
   ros::spinOnce();
   count++;
   loop_rate.sleep();

}

return 0;
}