keyboard should work ands show /cmd_vel
This commit is contained in:
parent
bd757b23ea
commit
7d3e20e3d9
3 changed files with 62 additions and 8 deletions
|
@ -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);
|
||||||
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
Loading…
Reference in a new issue