Merge branch 'master' of https://bitbucket.org/_Luc_/hand_control
This commit is contained in:
_Luc_
commit
336f4bdb76
8 changed files with 399 additions and 91 deletions
|
|
@ -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)
|
||||
|
|
|
|||
28
README.md
28
README.md
|
|
@ -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 :
|
||||
|
|
@ -65,4 +55,4 @@ Avant de coder, regarder :
|
|||
- [les conventions d’écriture du code du projet ROS](http://wiki.ros.org/CppStyleGuide)
|
||||
- [le guide du développeur](http://wiki.ros.org/DevelopersGuide)
|
||||
|
||||
Cf. le [Wiki](https://bitbucket.org/_Luc_/handcontrol/wiki/Home) pour le reste de la documentation et le résultat des recherches.
|
||||
Cf. le [Wiki](https://bitbucket.org/_Luc_/handcontrol/wiki/Home) pour le reste de la documentation et le résultat des recherches.
|
||||
|
|
|
|||
|
|
@ -1,3 +1,4 @@
|
|||
Header header
|
||||
geometry_msgs/Point normal
|
||||
float64 altitude
|
||||
float64 curvature
|
||||
|
|
|
|||
206
src/display.cpp
Normal file
206
src/display.cpp
Normal 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
55
src/display.h
Normal 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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in a new issue