Offboard control Node Examples

[Jaeyoung-Lim]

Here is a offboard control node example

//Conversion Node

include <ros/ros.h>

include <std_msgs/String.h>

include

include "geometry_msgs/PoseStamped.h"

include "geometry_msgs/TwistStamped.h"

include "geometry_msgs/TransformStamped.h"

geometry_msgs::PoseStamped msg_pose; geometry_msgs::TwistStamped msg_vel; double x_pre= 0.0, y_pre = 0.0, z_pre = 0.0; double v_x_fil = 0.0, v_y_fil = 0.0, v_z_fil = 0.0; double t_p = 0.0, t_n = 0.0, dt;

void Callback(const geometry_msgs::TransformStamped msg) { t_p = t_n; t_n = ros::Time::now().toSec(); dt = t_n - t_p;

x_pre = msg_pose.pose.position.x;
y_pre = msg_pose.pose.position.y;
z_pre = msg_pose.pose.position.z;
msg_pose.pose.position.x = msg.transform.translation.x;
msg_pose.pose.position.y = msg.transform.translation.y;
msg_pose.pose.position.z = msg.transform.translation.z;

msg_pose.pose.orientation.x = msg.transform.rotation.x;
msg_pose.pose.orientation.y = msg.transform.rotation.y;
msg_pose.pose.orientation.z = msg.transform.rotation.z;
msg_pose.pose.orientation.w = msg.transform.rotation.w;

double gain = 0.5;
v_x_fil = gain*v_x_fil + (1-gain)*(msg_pose.pose.position.x-x_pre)/dt;
v_y_fil = gain*v_y_fil + (1-gain)*(msg_pose.pose.position.y-y_pre)/dt;
v_z_fil = gain*v_z_fil + (1-gain)*(msg_pose.pose.position.z-z_pre)/dt;

msg_vel.twist.linear.x = v_x_fil;
msg_vel.twist.linear.y = v_y_fil;
msg_vel.twist.linear.z = v_z_fil;

}

int main(int argc, char **argv) { ros::init(argc, argv, "conversion_node"); ros::NodeHandle n; ros::Subscriber pose_sub = n.subscribe("/controller/measurements",240, Callback); ros::Publisher pose_pub = n.advertise("/controller/controller_node/pose",480); ros::Publisher vel_pub = n.advertise("/controller/controller_node/vel",480); ros::Rate loop_rate(480); ros::spinOnce();

int count = 0; while (ros::ok()) { msg_pose.header.stamp = ros::Time::now(); msg_pose.header.seq = count; msg_pose.header.frame_id = 1;

pose_pub.publish(msg_pose);
vel_pub.publish(msg_vel);
ros::spinOnce();
loop_rate.sleep();
++count;

[Jaeyoung-Lim]

include <ros/ros.h>

include <std_msgs/String.h>

include

include "geometry_msgs/PoseStamped.h"

include <std_msgs/Float64.h>

include

include

include

include <sys/select.h>

include

include

include

// function prototypes for kbhit() static struct termios initial_settings, new_settings; static int peek_character = -1; int _kbhit(); int _getch(); void init_keyboard(); void close_keyboard();

// PI constant double PI = 3.1415926535897932384626433832795028841971;

int main(int argc, char **argv) { ros::init(argc, argv, "command_node"); ros::NodeHandle n;

ros::Publisher rpy_pub = n.advertise("/mavros/setpoint_attitude/attitude",1000); ros::Publisher thrust_pub = n.advertise("/mavros/setpoint_attitude/att_throttle",1000); ros::Rate loop_rate(1000);

int count = 0; double r = 0.0, p = 0.0, y = 0.0, thrust = 0.0;

init_keyboard();

while(ros::ok()){ geometry_msgs::PoseStamped msg_rpy; std_msgs::Float64 msg_thrust;

msg_rpy.header.stamp = ros::Time::now();
    msg_rpy.header.seq = count;
    msg_rpy.header.frame_id = 1;

if (_kbhit()) 
    {
    int input = _getch();
    std::cout<<"beep :"<<input<<std::endl;
    if (input == 'q' || input == 'Q') 
    {
        // quit
        std::cout << "\nExit by keyboard input q" << std::endl;
        close_keyboard();       // close detecting kbhit()
        usleep(50000);
        // quit main function (quit process)
        return 0;                   
    }
    else if(input == 'R')
    {
        r = r + PI/180.0*10.0;
    }
    else if(input == 'r')http://www.naver.com/
    {
        r = r - PI/180.0*10.0;
    }
    else if(input == 'P')
    {
        p = p + PI/180.0*10.0;
    }
    else if(input == 'p')
    {
        p = p - PI/180.0*10.0;
    }
    else if(input == 'Y')
    {
        y = y + PI/180.0*10.0;
    }
    else if(input == 'y')
    {
        y = y - PI/180.0*10.0;
    }
    else if(input == 'T')
    {
        thrust = thrust + 0.01;
    }
    else if(input == 't')
    {
        thrust = thrust - 0.01;
    }

    if (r<0.0000001 && r>-0.0000001){r = 0.0;}
    if (p<0.0000001 && p>-0.0000001){p = 0.0;}
    if (y<0.0000001 && y>-0.0000001){y = 0.0;}
    if (thrust<0.0000001 && thrust>-0.0000001){thrust = 0.0;}

}

if (thrust>=1.0)
{thrust = 1.0;}
else if (thrust<=0.0)
{thrust = 0.0;}

// attitude control
// euler zyx to quaternion
    msg_rpy.pose.orientation.x = sin(r/2.0)*cos(p/2.0)*cos(y/2.0) - cos(r/2.0)*sin(p/2.0)*sin(y/2.0);
    msg_rpy.pose.orientation.y = cos(r/2.0)*sin(p/2.0)*cos(y/2.0) + sin(r/2.0)*cos(p/2.0)*sin(y/2.0);
    msg_rpy.pose.orientation.z = cos(r/2.0)*cos(p/2.0)*sin(y/2.0) - sin(r/2.0)*sin(p/2.0)*cos(y/2.0);
    msg_rpy.pose.orientation.w = cos(r/2.0)*cos(p/2.0)*cos(y/2.0) + sin(r/2.0)*sin(p/2.0)*sin(y/2.0);

msg_thrust.data = thrust;

if (count % 10 == 0)

{ std::cout << "roll : " <<" "<< r_180/PI <<" " << "pitch : " <<" "<< p_180/PI <<" " << "yaw : " <<" "<< y*180/PI <<" "<< "thrust : "<<" " << thrust <<" "<< "quaternion : " << " "<< msg_rpy.pose.orientation.w<< " "<< msg_rpy.pose.orientation.x<< " "<< msg_rpy.pose.orientation.y<< " "<< msg_rpy.pose.orientation.z << std::endl; }

    rpy_pub.publish(msg_rpy);
    thrust_pub.publish(msg_thrust);
    ros::spinOnce();
    loop_rate.sleep();
    count++;

}

return 0; }

// Keyboard input // kbhit funtion int _kbhit() { unsigned char ch; int nread;

if (peek_character != -1)
    return 1;
new_settings.c_cc[VMIN] = 0;
tcsetattr(0, TCSANOW, &new_settings);
nread = read(0, &ch, 1);
new_settings.c_cc[VMIN] = 1;
tcsetattr(0, TCSANOW, &new_settings);
if (nread == 1) {
    peek_character = ch;
    return 1;
}
return 0;

}

int _getch() { char ch;

if (peek_character != -1) {
    ch = peek_character;
    peek_character = -1;
    return ch;
}
read(0, &ch, 1);
return ch;

}

void init_keyboard() { tcgetattr(0, &initial_settings); new_settings = initial_settings; new_settings.c_lflag &= ~ICANON; new_settings.c_lflag &= ~ECHO; new_settings.c_cc[VMIN] = 1; new_settings.c_cc[VTIME] = 0; tcsetattr(0, TCSANOW, &new_settings); }

void close_keyboard() { tcsetattr(0, TCSANOW, &initial_settings); }

[Jaeyoung-Lim]

include <ros/ros.h>

include <std_msgs/String.h>

include

[Jaeyoung-Lim]

include ros/ros.h>

include std_msgs/String.h>

include stdio.h>