Merge branch 'cppkeyboard'

This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-02 22:14:53 +02:00
commit 0ab7fc0bdf
4 changed files with 367 additions and 66 deletions

View file

@ -8,7 +8,13 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs std_msgs
geometry_msgs geometry_msgs
message_generation message_generation
ardrone_autonomy
) )
find_package(
PkgConfig REQUIRED
)
pkg_check_modules ( ncursesw REQUIRED ncursesw)
add_message_files( add_message_files(
FILES FILES
@ -27,6 +33,7 @@ CATKIN_DEPENDS message_runtime
include_directories( include_directories(
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
${ncursesw_INCLUDE_DIRS}
) )
add_executable(filtre src/filtre.cpp) add_executable(filtre src/filtre.cpp)
@ -38,6 +45,11 @@ target_link_libraries(random_pcl_publisher ${catkin_LIBRARIES})
add_executable(pcl_displayer src/pcl_displayer.cpp) add_executable(pcl_displayer src/pcl_displayer.cpp)
target_link_libraries(pcl_displayer ${catkin_LIBRARIES}) target_link_libraries(pcl_displayer ${catkin_LIBRARIES})
add_library(display src/display.cpp)
add_executable(keyboard_cmd src/keyboard_cmd.cpp)
target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRARIES})
add_executable(normal_estimator src/normal_estimator.cpp) add_executable(normal_estimator src/normal_estimator.cpp)
target_link_libraries(normal_estimator ${catkin_LIBRARIES}) target_link_libraries(normal_estimator ${catkin_LIBRARIES})
add_dependencies(normal_estimator hand_control_generate_messages_cpp) add_dependencies(normal_estimator hand_control_generate_messages_cpp)

206
src/display.cpp Normal file
View file

@ -0,0 +1,206 @@
#include <ncurses.h>
#include <string>
#include <geometry_msgs/Twist.h>
#include "display.h"
const int Curses::cmd_kbd_lines = 8;
const int Curses::cmd_kbd_columns = 55;
const int Curses::cmd_speed_lines = 4;
const int Curses::cmd_speed_columns = 55;
const int Curses::get_lines = 1;
const int Curses::get_columns = 1;
const int Curses::nav_data_lines = 3;
const int Curses::nav_data_columns = 25;
const int Curses::log_sent_w_lines = 12;
const int Curses::log_sent_w_columns = 22;
const int Curses::topic_lines = 8;
const int Curses::topic_columns = 22;
Curses::Curses() {
initscr();
cbreak();
start_color();
//noecho();
cmd_kbd = newwin(cmd_kbd_lines, cmd_kbd_columns, 0, 0);
get = newwin(get_lines, get_columns,
cmd_kbd_lines, cmd_kbd_columns/2);
cmd_speed = newwin(cmd_speed_lines, cmd_speed_columns,
cmd_kbd_lines + get_lines, 0);
log_sent_title = newwin(1, log_sent_w_columns,
0, cmd_kbd_columns + 1);
waddstr(log_sent_title, "SENT COMMANDS");
wrefresh(log_sent_title);
log_sent_w = newwin(log_sent_w_lines - 1, log_sent_w_columns,
1, cmd_kbd_columns + 1);
log_line_number = log_sent_w_lines - 2;
wattron(log_sent_w, A_BOLD);
init_pair(1, COLOR_GREEN, COLOR_BLACK);
wattron(log_sent_w, COLOR_PAIR(1));
scrollok(log_sent_w, TRUE);
nav_data = newwin(nav_data_lines, nav_data_columns,
cmd_kbd_lines + get_lines + cmd_speed_lines + 1, 0);
init_pair(2, COLOR_RED, COLOR_BLACK);
wattron(nav_data, COLOR_PAIR(2));
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_cmd_kbd();
print_cmd_speed();
print_topic();
wmove(get, 0, 0);
wrefresh(get);
}
Curses::~Curses() {
delwin(cmd_kbd);
delwin(cmd_speed);
delwin(log_sent_w);
delwin(log_sent_title);
delwin(nav_data);
delwin(get);
delwin(topic);
endwin();
}
char Curses::getchar() {
return wgetch(get);
}
void Curses::print_cmd_kbd() {
wmove(cmd_kbd, 0, 0);
waddstr(cmd_kbd, " ---------------------\n");
waddstr(cmd_kbd, " takeoff>| t|⇑ y|↖ u|↑ i|↗ o|\n");
waddstr(cmd_kbd, " |---|---|---|---|---|----\n");
waddstr(cmd_kbd, " reset>| g|⇐ h|← j| k|→ l|⇒ m|\n");
waddstr(cmd_kbd, " |---|---|---|---|---|----\n");
waddstr(cmd_kbd, " land>| b|⇓ n|↙ ,|↓ ;|↘ :|\n");
waddstr(cmd_kbd, " ---------------------\n");
waddstr(cmd_kbd, " Press ctrl + C to quit");
wrefresh(cmd_kbd);
}
void Curses::print_cmd_speed() {
wmove(cmd_speed, 0, 0);
waddstr(cmd_speed, " `x` cmd speed : (a/w : increase/decrease)\n");
waddstr(cmd_speed, " `y` cmd speed : (z/x : increase/decrease)\n");
waddstr(cmd_speed, " `z` cmd speed : (e/c : increase/decrease)\n");
waddstr(cmd_speed, "rotation speed : (r/v : increase/decrease)\n");
wrefresh(cmd_speed);
}
void Curses::update_cmd_speed(const char& coord, const float& v) {
switch(coord) {
case 'x':
wmove(cmd_speed, 0, 16);
wprintw(cmd_speed, "%f", v);
break;
case 'y':
wmove(cmd_speed, 1, 16);
wprintw(cmd_speed, "%f", v);
break;
case 'z':
wmove(cmd_speed, 2, 16);
wprintw(cmd_speed, "%f", v);
break;
case 't':
wmove(cmd_speed, 3, 16);
wprintw(cmd_speed, "%f", v);
break;
default:
;
}
wrefresh(cmd_speed);
}
void Curses::log_sent(const std::string& str) {
wmove(log_sent_w, log_line_number++, 0);
waddstr(log_sent_w, (str + "\n").c_str() );
wrefresh(log_sent_w);
}
void Curses::print_nav_data() {
wmove(nav_data, 0, 0);
waddstr(nav_data, "Battery :\n State :\n Time :");
wrefresh(nav_data);
}
void Curses::update_nav_data(const float& batteryPercent,
const int& state,
const float& time) {
wmove(nav_data, 0, 10);
wprintw(nav_data, "%f %%", batteryPercent);
wmove(nav_data, 2, 10);
wprintw(nav_data, "%f %", time);
wmove(nav_data, 1, 10);
switch(state) {
case 0:
waddstr(nav_data, "unknown ");
break;
case 1:
waddstr(nav_data, "inited ");
break;
case 2:
waddstr(nav_data, "landed ");
break;
case 3:
waddstr(nav_data, "flying ");
break;
case 4:
waddstr(nav_data, "hovering ");
break;
case 5:
waddstr(nav_data, "test ");
break;
case 6:
waddstr(nav_data, "taking off ");
break;
case 7:
waddstr(nav_data, "flying ");
break;
case 8:
waddstr(nav_data, "landing ");
break;
case 9:
waddstr(nav_data, "looping ");
break;
default:
;
}
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);
}

55
src/display.h Normal file
View file

@ -0,0 +1,55 @@
#ifndef CURSES_DISPLAY
#define CURSES_DISPLAY
#include <ncurses.h>
#include <string>
#include <geometry_msgs/Twist.h>
class Curses
{
private:
static const int cmd_kbd_lines;
static const int cmd_kbd_columns;
WINDOW* cmd_kbd;
void print_cmd_kbd();
static const int cmd_speed_lines;
static const int cmd_speed_columns;
WINDOW* cmd_speed;
void print_cmd_speed();
static const int get_lines;
static const int get_columns;
WINDOW* get;
static const int log_sent_w_lines;
static const int log_sent_w_columns;
int log_line_number;
WINDOW* log_sent_w;
WINDOW* log_sent_title;
static const int nav_data_lines;
static const int nav_data_columns;
WINDOW* nav_data;
void print_nav_data();
static const int topic_lines;
static const int topic_columns;
WINDOW* topic;
void print_topic();
public:
Curses();
~Curses();
char getchar();
// TODO
void update_cmd_speed(const char& coord, const float& v);
void update_nav_data(const float& batteryPercent,
const int& state,
const float& time);
void log_sent(const std::string& str);
void update_topic(const geometry_msgs::Twist::ConstPtr& twist);
};
#endif

View file

@ -1,15 +1,40 @@
#include <termios.h>
#include <iostream>
#include <map>
#include <vector>
#include <string>
#include <ros/ros.h> #include <ros/ros.h>
#include <ros/time.h> #include <ros/time.h>
#include <locale.h>
#include <ncurses.h> #include <ncurses.h>
#include "display.h"
#include <std_msgs/Empty.h> #include <std_msgs/Empty.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <ardrone_autonomy/Navdata.h>
class NavdataCallback
{
private:
boost::shared_ptr<Curses> term;
public:
NavdataCallback(const boost::shared_ptr<Curses>& terminal) :
term(terminal) {}
void operator()(const ardrone_autonomy::Navdata::ConstPtr& msg) {
term->update_nav_data(msg->batteryPercent, msg->state, msg->tm);
}
}; // 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
{ {
@ -18,24 +43,20 @@ 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;
void land() ros::Subscriber data_subscriber, cmdvel_subscriber;
{ pub_land.publish(empty); }; void land() { pub_land.publish(empty); };
void takeoff() void takeoff() { pub_takeoff.publish(empty); };
{ 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;
void print_speed() boost::shared_ptr<Curses> term;
{ NavdataCallback data_callback;
std::cout << "x speed : " << x_speed << ", y speed : " << CmdVelCallback cmdvel_callback;
y_speed << ", z speed : " << z_speed <<
", rotation speed : " << turn << "\n";
};
public: public:
Run() : Run(const boost::shared_ptr<Curses>& terminal) :
n(), data_callback(terminal),
empty(), cmdvel_callback(terminal),
term(terminal),
loop_rate(30), loop_rate(30),
x_speed(0.2), x_speed(0.2),
y_speed(0.3), y_speed(0.3),
@ -45,85 +66,80 @@ class Run
pub_takeoff = n.advertise<std_msgs::Empty>("/ardrone/takeoff", 1); pub_takeoff = n.advertise<std_msgs::Empty>("/ardrone/takeoff", 1);
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);
cmdvel_subscriber = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, cmdvel_callback);
~Run() term->update_cmd_speed('x', x_speed);
{ term->update_cmd_speed('y', y_speed);
endwin(); term->update_cmd_speed('z', z_speed);
} term->update_cmd_speed('t', turn);
float a(0);
int s(0);
float time(0);
term->update_nav_data(a, s, time);
}
void operator()() void operator()()
{ {
/*
std::cout
<<" ---------------------\n"
<<"takeoff>| t|⇑ y|↖ u|↑ i|↗ o|\n"
<<" |---|---|---|---|---|----\n"
<<" reset>| g|⇐ h|← j| k|→ l|⇒ m|\n"
<<" |---|---|---|---|---|----\n"
<<" land>| b|⇓ n|↙ ,|↓ ;|↘ :|\n"
<<" ---------------------\n"
<<"\n"
<<"a/w : increase/decrease linear `x` speeds by 10%\n"
<<"z/x : increase/decrease linear `y` speed by 10%\n"
<<"e/c : increase/decrease linear `z` speed by 10%\n"
<<"r/v : increase/decrease rotation speed by 10%\n";
*/
initscr();
cbreak();
noecho();
while (ros::ok()) while (ros::ok())
{ {
ros::spinOnce();
geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist()); geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist());
msg->linear.x = msg->linear.y = msg->linear.z = msg->linear.x = msg->linear.y = msg->linear.z =
msg->angular.x = msg->angular.y = msg->angular.z = 0.; msg->angular.x = msg->angular.y = msg->angular.z = 0.;
char c = getch(); char c = term->getchar();
switch(c) switch(c)
{ {
case 'k' : case 'k' :
{// hover {// hover
cmd.publish(msg); cmd.publish(msg);
term->log_sent("hover !");
break; break;
} }
case 'i' : case 'i' :
{// forward {// forward
msg->linear.x = x_speed; msg->linear.x = x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("forward !");
break; break;
} }
case ';' : case ';' :
{// backward {// backward
msg->linear.x = -x_speed; msg->linear.x = -x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("backward !");
break; break;
} }
case 'h' : case 'h' :
{//translate left {//translate left
msg->linear.y = -y_speed; msg->linear.y = -y_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("translate left !");
break; break;
} }
case 'm' : case 'm' :
{//translate right {//translate right
msg->linear.y = y_speed; msg->linear.y = y_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("translate right !");
break; break;
} }
case 'j' : case 'j' :
{//rotate left {//rotate left
msg->angular.z = turn; msg->angular.z = turn;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("rotate left !");
break; break;
} }
case 'l' : case 'l' :
{//rotate right {//rotate right
msg->angular.z = -turn; msg->angular.z = -turn;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("rotate right !");
break; break;
} }
case 'u' : case 'u' :
@ -131,6 +147,7 @@ class Run
msg->angular.z = turn; msg->angular.z = turn;
msg->linear.x = x_speed; msg->linear.x = x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("forward left !");
break; break;
} }
case 'o' : case 'o' :
@ -138,6 +155,7 @@ class Run
msg->angular.z = -turn; msg->angular.z = -turn;
msg->linear.x = x_speed; msg->linear.x = x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("forward right !");
break; break;
} }
case ',' : case ',' :
@ -145,6 +163,7 @@ class Run
msg->angular.z = turn; msg->angular.z = turn;
msg->linear.x = -x_speed; msg->linear.x = -x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("backward left !");
break; break;
} }
case ':' : case ':' :
@ -152,99 +171,108 @@ class Run
msg->angular.z = -turn; msg->angular.z = -turn;
msg->linear.x = -x_speed; msg->linear.x = -x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("backward right !");
break; break;
} }
case 'y' : case 'y' :
{//up {//up
msg->linear.z = z_speed; msg->linear.z = z_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("up !");
break; break;
} }
case 'n' : case 'n' :
{//down {//down
msg->linear.z = -z_speed; msg->linear.z = -z_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("down !");
break; break;
} }
case 't' : case 't' :
{//takeoff {//takeoff
takeoff(); takeoff();
term->log_sent("takeoff !");
break; break;
} }
case 'b' : case 'b' :
{//land {//land
land(); land();
term->log_sent("land !");
break; break;
} }
case 'g' : case 'g' :
{//reset {//reset
reset(); reset();
term->log_sent("reset !");
break; break;
} }
case 'a' : case 'a' :
{// + x_speed {// + x_speed
x_speed *= 1.1; x_speed *= 1.1;
print_speed(); term->update_cmd_speed('x', x_speed);
break; break;
} }
case 'w' : case 'w' :
{// - x_speed {// - x_speed
x_speed *= 0.9; x_speed *= 0.9;
print_speed(); term->update_cmd_speed('x', x_speed);
break; break;
} }
case 'z' : case 'z' :
{// + y_speed {// + y_speed
y_speed *= 1.1; y_speed *= 1.1;
print_speed(); term->update_cmd_speed('y', y_speed);
break; break;
} }
case 'x' : case 'x' :
{// - y_speed {// - y_speed
y_speed *= 0.9; y_speed *= 0.9;
print_speed(); term->update_cmd_speed('y', y_speed);
break; break;
} }
case 'e' : case 'e' :
{// + z_speed {// + z_speed
z_speed *= 1.1; z_speed *= 1.1;
print_speed(); term->update_cmd_speed('z', z_speed);
break; break;
} }
case 'c' : case 'c' :
{// - z_speed {// - z_speed
z_speed *= 0.9; z_speed *= 0.9;
print_speed(); term->update_cmd_speed('z', z_speed);
break; break;
} }
case 'r' : case 'r' :
{// + turn speed {// + turn speed
turn *= 1.1; turn *= 1.1;
print_speed(); term->update_cmd_speed('t', turn);
break; break;
} }
case 'v' : case 'v' :
{// - turn speed {// - turn speed
turn *= 0.9; turn *= 0.9;
print_speed(); term->update_cmd_speed('t', turn);
break; break;
} }
default : default :
break; {
} // switch cmd.publish(msg);
term->log_sent("hover !");
ros::spinOnce(); }
} // switch(c)
loop_rate.sleep(); loop_rate.sleep();
} // while } // while
} // run } //void run()
}; // class
}; // class Run
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
setlocale(LC_ALL, "");
ros::init(argc, argv, "keyboard_cmd"); ros::init(argc, argv, "keyboard_cmd");
Run run; boost::shared_ptr<Curses> term(new Curses());
run(); Run fun(term);
fun();
return 0; return 0;
} } // main