keyboard should work ands show /cmd_vel

This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-02 22:14:03 +02:00
parent bd757b23ea
commit 7d3e20e3d9
3 changed files with 62 additions and 8 deletions

View file

@ -1,5 +1,6 @@
#include <ncurses.h> #include <ncurses.h>
#include <string> #include <string>
#include <geometry_msgs/Twist.h>
#include "display.h" #include "display.h"
const int Curses::cmd_kbd_lines = 8; const int Curses::cmd_kbd_lines = 8;
@ -12,10 +13,13 @@ const int Curses::get_lines = 1;
const int Curses::get_columns = 1; const int Curses::get_columns = 1;
const int Curses::nav_data_lines = 3; const int Curses::nav_data_lines = 3;
const int Curses::nav_data_columns = 55; const int Curses::nav_data_columns = 25;
const int Curses::log_sent_w_lines = 12; const int Curses::log_sent_w_lines = 12;
const int Curses::log_sent_w_columns = 40; const int Curses::log_sent_w_columns = 22;
const int Curses::topic_lines = 8;
const int Curses::topic_columns = 22;
Curses::Curses() { Curses::Curses() {
initscr(); initscr();
@ -49,9 +53,17 @@ Curses::Curses() {
wattron(nav_data, COLOR_PAIR(2)); wattron(nav_data, COLOR_PAIR(2));
wattron(nav_data, A_BOLD); wattron(nav_data, A_BOLD);
topic = newwin(topic_lines, topic_columns,
cmd_kbd_lines + get_lines + cmd_speed_lines + 1,
nav_data_columns + 1);
init_pair(3, COLOR_BLUE, COLOR_BLACK);
wattron(topic, COLOR_PAIR(3));
wattron(topic, A_BOLD);
print_nav_data(); print_nav_data();
print_cmd_kbd(); print_cmd_kbd();
print_cmd_speed(); print_cmd_speed();
print_topic();
wmove(get, 0, 0); wmove(get, 0, 0);
wrefresh(get); wrefresh(get);
@ -64,6 +76,7 @@ Curses::~Curses() {
delwin(log_sent_title); delwin(log_sent_title);
delwin(nav_data); delwin(nav_data);
delwin(get); delwin(get);
delwin(topic);
endwin(); endwin();
} }
@ -134,7 +147,7 @@ void Curses::update_nav_data(const float& batteryPercent,
const int& state, const int& state,
const float& time) { const float& time) {
wmove(nav_data, 0, 10); wmove(nav_data, 0, 10);
wprintw(nav_data, "%f %", batteryPercent); wprintw(nav_data, "%f %%", batteryPercent);
wmove(nav_data, 2, 10); wmove(nav_data, 2, 10);
wprintw(nav_data, "%f %", time); wprintw(nav_data, "%f %", time);
wmove(nav_data, 1, 10); wmove(nav_data, 1, 10);
@ -174,3 +187,20 @@ void Curses::update_nav_data(const float& batteryPercent,
} }
wrefresh(nav_data); wrefresh(nav_data);
} }
void Curses::print_topic() {
wmove(topic, 0, 0);
waddstr(topic, "Linear :\n x : \n y : \n z : \n");
waddstr(topic, "Angular :\n x : \n y : \n z : ");
wrefresh(topic);
}
void Curses::update_topic(const geometry_msgs::Twist::ConstPtr& twist) {
wmove(topic, 1, 5); wprintw(topic, "%f ", twist->linear.x);
wmove(topic, 2, 5); wprintw(topic, "%f ", twist->linear.y);
wmove(topic, 3, 5); wprintw(topic, "%f ", twist->linear.z);
wmove(topic, 5, 5); wprintw(topic, "%f ", twist->angular.x);
wmove(topic, 6, 5); wprintw(topic, "%f ", twist->angular.y);
wmove(topic, 7, 5); wprintw(topic, "%f ", twist->angular.z);
wrefresh(topic);
}

View file

@ -3,6 +3,7 @@
#include <ncurses.h> #include <ncurses.h>
#include <string> #include <string>
#include <geometry_msgs/Twist.h>
class Curses class Curses
{ {
@ -32,6 +33,11 @@ class Curses
WINDOW* nav_data; WINDOW* nav_data;
void print_nav_data(); void print_nav_data();
static const int topic_lines;
static const int topic_columns;
WINDOW* topic;
void print_topic();
public: public:
Curses(); Curses();
~Curses(); ~Curses();
@ -43,6 +49,7 @@ class Curses
const int& state, const int& state,
const float& time); const float& time);
void log_sent(const std::string& str); void log_sent(const std::string& str);
void update_topic(const geometry_msgs::Twist::ConstPtr& twist);
}; };
#endif #endif

View file

@ -8,19 +8,33 @@
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <ardrone_autonomy/Navdata.h> #include <ardrone_autonomy/Navdata.h>
class Callback class NavdataCallback
{ {
private: private:
boost::shared_ptr<Curses> term; boost::shared_ptr<Curses> term;
public: public:
Callback(const boost::shared_ptr<Curses>& terminal) : NavdataCallback(const boost::shared_ptr<Curses>& terminal) :
term(terminal) {} term(terminal) {}
void operator()(const ardrone_autonomy::Navdata::ConstPtr& msg) { void operator()(const ardrone_autonomy::Navdata::ConstPtr& msg) {
term->update_nav_data(msg->batteryPercent, msg->state, msg->tm); term->update_nav_data(msg->batteryPercent, msg->state, msg->tm);
} }
}; // class Callback }; // class NavdataCallback
class CmdVelCallback
{
private:
boost::shared_ptr<Curses> term;
public:
CmdVelCallback(const boost::shared_ptr<Curses>& terminal) :
term(terminal) {}
void operator()(const geometry_msgs::Twist::ConstPtr& msg) {
term->update_topic(msg);
}
}; // class CmdVelCallback
class Run class Run
{ {
@ -29,17 +43,19 @@ class Run
ros::NodeHandle n; ros::NodeHandle n;
ros::Rate loop_rate; ros::Rate loop_rate;
ros::Publisher cmd, pub_takeoff, pub_land, pub_reset; ros::Publisher cmd, pub_takeoff, pub_land, pub_reset;
ros::Subscriber data_subscriber; ros::Subscriber data_subscriber, cmdvel_subscriber;
void land() { pub_land.publish(empty); }; void land() { pub_land.publish(empty); };
void takeoff() { pub_takeoff.publish(empty); }; void takeoff() { pub_takeoff.publish(empty); };
void reset() { pub_reset.publish(empty); }; void reset() { pub_reset.publish(empty); };
float x_speed, y_speed, z_speed, turn; float x_speed, y_speed, z_speed, turn;
boost::shared_ptr<Curses> term; boost::shared_ptr<Curses> term;
Callback data_callback; NavdataCallback data_callback;
CmdVelCallback cmdvel_callback;
public: public:
Run(const boost::shared_ptr<Curses>& terminal) : Run(const boost::shared_ptr<Curses>& terminal) :
data_callback(terminal), data_callback(terminal),
cmdvel_callback(terminal),
term(terminal), term(terminal),
loop_rate(30), loop_rate(30),
x_speed(0.2), x_speed(0.2),
@ -51,6 +67,7 @@ class Run
pub_land = n.advertise<std_msgs::Empty>("/ardrone/land", 1); pub_land = n.advertise<std_msgs::Empty>("/ardrone/land", 1);
pub_reset = n.advertise<std_msgs::Empty>("/ardrone/reset", 1); pub_reset = n.advertise<std_msgs::Empty>("/ardrone/reset", 1);
data_subscriber = n.subscribe<ardrone_autonomy::Navdata>("/ardrone/navdata", 1, data_callback); data_subscriber = n.subscribe<ardrone_autonomy::Navdata>("/ardrone/navdata", 1, data_callback);
cmdvel_subscriber = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, cmdvel_callback);
term->update_cmd_speed('x', x_speed); term->update_cmd_speed('x', x_speed);
term->update_cmd_speed('y', y_speed); term->update_cmd_speed('y', y_speed);