LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Merge branch 'master' of https://bitbucket.org/_Luc_/hand_control

Browse source
This commit is contained in:
_Luc_ 2015-05-06 15:54:53 +02:00
commit 336f4bdb76
8 changed files with 399 additions and 91 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

16
CMakeLists.txt
View file

@ -8,7 +8,13 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
geometry_msgs
message_generation
ardrone_autonomy
)
find_package(
PkgConfig REQUIRED
)
pkg_check_modules ( ncursesw REQUIRED ncursesw)
add_message_files(
FILES
@ -21,10 +27,13 @@ generate_messages(
geometry_msgs
)
catkin_package()
catkin_package(
CATKIN_DEPENDS message_runtime
)
include_directories(
${catkin_INCLUDE_DIRS}
${ncursesw_INCLUDE_DIRS}
)
add_executable(filtre src/filtre.cpp)
@ -36,6 +45,11 @@ target_link_libraries(random_pcl_publisher ${catkin_LIBRARIES})
add_executable(pcl_displayer src/pcl_displayer.cpp)
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)
target_link_libraries(normal_estimator ${catkin_LIBRARIES})
add_dependencies(normal_estimator hand_control_generate_messages_cpp)

26
README.md
View file

@ -1,29 +1,20 @@
# IMPORTANT #
Changement d’url du dépôt. Exécutez la commande ci-dessous à la racine de votre dépôt local :
```
#!sh
git remote set-url origin git@bitbucket.org:_Luc_/hand_control.git
```
ou :
```
#!sh
git remote set-url origin https://username@bitbucket.org/_Luc_/hand_control.git # changer "username"
```
# Contrôle par geste d'un drone #
**Extrait de la présentation du projet**
« On s'intéresse dans ce projet à contrôler un drone à l'aide la main. On utilisera pour ce faire une kinect, placée à l'horizontal, au dessus de laquelle on placera la main du contrôleur. La kinect fournit des informations sur la profondeur des objets placés en face d'elle. On peut alors régresser un plan sur les échantillons et utiliser deux inclinaisons et la distance pour contrôler le roulis, le tangage et l'altitude d'un drone. »
## Installation des dépendances ##
```
#!sh
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | sudo apt-key add -
sudo apt-get update
sudo apt-get install ros-indigo-desktop-full ros-indigo-freenect-stack ros-indigo-ardrone-autonomy libncursesw5-dev
```
## Utilisation du dépôt ##
Après avoir créé un espace de travail catkin :
```
#!sh
source /opt/ros/indigo/setup.bash
@ -45,7 +36,6 @@ Le contenu du dépôt se trouve alors dans «~/hand_control_ws/src». Il est ens
#!sh
cd ~/hand_control_ws
catkin_make
catkin_make # régle le problème du message "Plan"
```
Puis pour faciliter le développement :

1
msg/Plan.msg
View file

@ -1,3 +1,4 @@
Header header
geometry_msgs/Point normal
float64 altitude
float64 curvature

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

161
src/keyboard_cmd.cpp
View file

@ -1,15 +1,39 @@
#include <termios.h>
#include <iostream>
#include <map>
#include <vector>
#include <string>
#include <ros/ros.h>
#include <ros/time.h>
#include <ncurses.h>
#include <locale.h>
#include "display.h"
#include <std_msgs/Empty.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
{
@ -18,24 +42,20 @@ class Run
ros::NodeHandle n;
ros::Rate loop_rate;
ros::Publisher cmd, pub_takeoff, pub_land, pub_reset;
void land()
{ pub_land.publish(empty); };
void takeoff()
{ pub_takeoff.publish(empty); };
void reset()
{ pub_reset.publish(empty); };
ros::Subscriber data_subscriber, cmdvel_subscriber;
void land() { pub_land.publish(empty); };
void takeoff() { pub_takeoff.publish(empty); };
void reset() { pub_reset.publish(empty); };
float x_speed, y_speed, z_speed, turn;
void print_speed()
{
std::cout << "x speed : " << x_speed << ", y speed : " <<
y_speed << ", z speed : " << z_speed <<
", rotation speed : " << turn << "\n";
};
boost::shared_ptr<Curses> term;
NavdataCallback data_callback;
CmdVelCallback cmdvel_callback;
public:
Run() :
n(),
empty(),
Run(const boost::shared_ptr<Curses>& terminal) :
data_callback(terminal),
cmdvel_callback(terminal),
term(terminal),
loop_rate(30),
x_speed(0.2),
y_speed(0.3),
@ -45,85 +65,80 @@ class Run
pub_takeoff = n.advertise<std_msgs::Empty>("/ardrone/takeoff", 1);
pub_land = n.advertise<std_msgs::Empty>("/ardrone/land", 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()
{
endwin();
}
term->update_cmd_speed('x', x_speed);
term->update_cmd_speed('y', y_speed);
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()()
{
/*
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())
{
ros::spinOnce();
geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist());
msg->linear.x = msg->linear.y = msg->linear.z =
msg->angular.x = msg->angular.y = msg->angular.z = 0.;
char c = getch();
char c = term->getchar();
switch(c)
{
case 'k' :
{// hover
cmd.publish(msg);
term->log_sent("hover !");
break;
}
case 'i' :
{// forward
msg->linear.x = x_speed;
cmd.publish(msg);
term->log_sent("forward !");
break;
}
case ';' :
{// backward
msg->linear.x = -x_speed;
cmd.publish(msg);
term->log_sent("backward !");
break;
}
case 'h' :
{//translate left
msg->linear.y = -y_speed;
cmd.publish(msg);
term->log_sent("translate left !");
break;
}
case 'm' :
{//translate right
msg->linear.y = y_speed;
cmd.publish(msg);
term->log_sent("translate right !");
break;
}
case 'j' :
{//rotate left
msg->angular.z = turn;
cmd.publish(msg);
term->log_sent("rotate left !");
break;
}
case 'l' :
{//rotate right
msg->angular.z = -turn;
cmd.publish(msg);
term->log_sent("rotate right !");
break;
}
case 'u' :
@ -131,6 +146,7 @@ class Run
msg->angular.z = turn;
msg->linear.x = x_speed;
cmd.publish(msg);
term->log_sent("forward left !");
break;
}
case 'o' :
@ -138,6 +154,7 @@ class Run
msg->angular.z = -turn;
msg->linear.x = x_speed;
cmd.publish(msg);
term->log_sent("forward right !");
break;
}
case ',' :
@ -145,6 +162,7 @@ class Run
msg->angular.z = turn;
msg->linear.x = -x_speed;
cmd.publish(msg);
term->log_sent("backward left !");
break;
}
case ':' :
@ -152,99 +170,108 @@ class Run
msg->angular.z = -turn;
msg->linear.x = -x_speed;
cmd.publish(msg);
term->log_sent("backward right !");
break;
}
case 'y' :
{//up
msg->linear.z = z_speed;
cmd.publish(msg);
term->log_sent("up !");
break;
}
case 'n' :
{//down
msg->linear.z = -z_speed;
cmd.publish(msg);
term->log_sent("down !");
break;
}
case 't' :
{//takeoff
takeoff();
term->log_sent("takeoff !");
break;
}
case 'b' :
{//land
land();
term->log_sent("land !");
break;
}
case 'g' :
{//reset
reset();
term->log_sent("reset !");
break;
}
case 'a' :
{// + x_speed
x_speed *= 1.1;
print_speed();
term->update_cmd_speed('x', x_speed);
break;
}
case 'w' :
{// - x_speed
x_speed *= 0.9;
print_speed();
term->update_cmd_speed('x', x_speed);
break;
}
case 'z' :
{// + y_speed
y_speed *= 1.1;
print_speed();
term->update_cmd_speed('y', y_speed);
break;
}
case 'x' :
{// - y_speed
y_speed *= 0.9;
print_speed();
term->update_cmd_speed('y', y_speed);
break;
}
case 'e' :
{// + z_speed
z_speed *= 1.1;
print_speed();
term->update_cmd_speed('z', z_speed);
break;
}
case 'c' :
{// - z_speed
z_speed *= 0.9;
print_speed();
term->update_cmd_speed('z', z_speed);
break;
}
case 'r' :
{// + turn speed
turn *= 1.1;
print_speed();
term->update_cmd_speed('t', turn);
break;
}
case 'v' :
{// - turn speed
turn *= 0.9;
print_speed();
term->update_cmd_speed('t', turn);
break;
}
default :
break;
} // switch
ros::spinOnce();
{
cmd.publish(msg);
term->log_sent("hover !");
}
} // switch(c)
loop_rate.sleep();
} // while
} // run
}; // class
} //void run()
}; // class Run
int main(int argc, char** argv)
{
setlocale(LC_ALL, "");
ros::init(argc, argv, "keyboard_cmd");
Run run;
run();
boost::shared_ptr<Curses> term(new Curses());
Run fun(term);
fun();
return 0;
}
} // main

15
src/normal_estimator.cpp
View file

@ -3,6 +3,7 @@
#include <pcl/point_types.h>
#include <pcl/features/normal_3d_omp.h>
#include <hand_control/Plan.h>
#include <time.h>
typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud;
@ -28,10 +29,11 @@ class Callback {
// publication
ROS_INFO("Plan published");
publisher.publish(to_Plan(x, y, z, h, c));
publisher.publish(to_Plan(x, y, z, h, c, msg->header.seq, msg->header.stamp));
}
Callback(ros::Publisher& pub) : publisher(pub), estimator() {}
Callback(ros::Publisher& pub) :
publisher(pub), estimator() {}
private:
ros::Publisher publisher;
@ -41,7 +43,7 @@ class Callback {
const hand_control::Plan::ConstPtr
to_Plan(const float& x, const float& y,
const float& z, const float& h,
const float& c)
const float& c, uint32_t seq, uint64_t msec64)
{
hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
ros_msg->normal.x = x;
@ -49,6 +51,13 @@ class Callback {
ros_msg->normal.z = z;
ros_msg->altitude = h;
ros_msg->curvature = c;
// uint64_t msec64 is in ms (10-6)
uint64_t sec64 = msec64 / 1000000;
uint64_t nsec64 = (msec64 % 1000000) * 1000;
ros_msg->header.stamp.sec = (uint32_t) sec64;
ros_msg->header.stamp.nsec = (uint32_t) nsec64;
ros_msg->header.seq = seq;
ros_msg->header.frame_id = "0";
return ros_msg;
}

8
src/random_pcl_publisher.cpp
View file

@ -1,4 +1,5 @@
#include <ros/ros.h>
#include <time.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
// for UniformGenerator
@ -13,7 +14,7 @@ class Generator
{
public:
Generator(int len, double m, double M)
: length(len), min(m), max(M), cgen()
: length(len), min(m), max(M), cgen(), number(0)
{
UGenerator::Parameters params(min, max, -1);
cgen.setParameters(params);
@ -30,6 +31,10 @@ class Generator
pcl->points[i].g = (uint8_t) 255;
pcl->points[i].b = (uint8_t) 0;
}
ros::Time now = ros::Time::now();
pcl->header.stamp = now.toNSec() / 1000;
pcl->header.seq = number++;
pcl->header.frame_id = "0";
return pcl;
}
@ -37,6 +42,7 @@ class Generator
pcl::common::CloudGenerator<pcl::PointXYZRGB, UGenerator> cgen;
int length;
double min, max;
uint32_t number;
};
int