Closed Jaeyoung-Lim closed 6 years ago
// 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
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); }
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;
}
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;