complies with cpplint
This commit is contained in:
Louis-Guillaume DUBOIS
parent
f94492264d
commit
15b11c3bb5
6 changed files with 635 additions and 648 deletions
|
|
@ -9,164 +9,150 @@
|
|||
*
|
||||
* Hand Control is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Hand Control. If not, see <http://www.gnu.org/licenses/>.
|
||||
* along with Hand Control. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <assert.h>
|
||||
#include <locale.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <ros/time.h>
|
||||
#include <locale.h>
|
||||
#include <limits>
|
||||
#include <math.h>
|
||||
#include <assert.h>
|
||||
|
||||
#include <pcl_ros/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
|
||||
#include <hand_control/Plan.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <hand_control/CommanderConfig.h>
|
||||
|
||||
#include <limits>
|
||||
|
||||
class Run
|
||||
{
|
||||
private:
|
||||
float xx, yy, zz, theta; // read coords
|
||||
|
||||
class Run {
|
||||
private:
|
||||
float xx, yy, zz, theta; // read coords
|
||||
|
||||
// see README.md to know what are the parameters
|
||||
float x_vel, y_vel, z_vel, angle_vel, up_factor, neutral_z; // parameters
|
||||
float max_curv; // not used yet
|
||||
float z_dev_min, x_dev_min, y_dev_min, th_dev_min; // parameters : thresholds
|
||||
uint64_t min_number; // parameter
|
||||
bool no_diag; // parameter
|
||||
float x_vel, y_vel, z_vel, angle_vel,
|
||||
up_factor, neutral_z; // parameters
|
||||
float max_curv; // not used yet
|
||||
float z_dev_min, x_dev_min, y_dev_min,
|
||||
th_dev_min; // parameters : thresholds
|
||||
uint64_t min_number; // parameter
|
||||
bool no_diag; // parameter
|
||||
|
||||
ros::Publisher pub;
|
||||
|
||||
void publish()
|
||||
// build and publish a message from the "xx", "yy", "zz" and "theta" informations
|
||||
{
|
||||
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
|
||||
void publish() {
|
||||
// build and publish a message from the "xx",
|
||||
// "yy", "zz" and "theta" informations
|
||||
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
|
||||
|
||||
if (fabs(zz) > z_dev_min)
|
||||
{
|
||||
// up_factor to balance out the gravity effect
|
||||
if (zz > 0)
|
||||
mvt->linear.z = zz * z_vel * up_factor ;
|
||||
else
|
||||
mvt->linear.z = zz * z_vel;
|
||||
}
|
||||
|
||||
|
||||
// no_diag true : the drone can only translate on the "x" axis
|
||||
// or the "y" axis but not on a linear combination of these axes.
|
||||
if (no_diag)
|
||||
{
|
||||
if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
|
||||
{
|
||||
mvt->linear.y = yy * y_vel;
|
||||
if (fabs(zz) > z_dev_min) {
|
||||
// up_factor to balance out the gravity effect
|
||||
if (zz > 0)
|
||||
mvt->linear.z = zz * z_vel * up_factor;
|
||||
else
|
||||
mvt->linear.z = zz * z_vel;
|
||||
}
|
||||
else if (fabs(xx) > x_dev_min)
|
||||
{
|
||||
mvt->linear.x = - xx * x_vel;
|
||||
|
||||
// no_diag true : the drone can only translate on the "x" axis
|
||||
// or the "y" axis but not on a linear combination of these axes.
|
||||
if (no_diag) {
|
||||
if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
|
||||
mvt->linear.y = yy * y_vel;
|
||||
else if (fabs(xx) > x_dev_min)
|
||||
mvt->linear.x = - xx * x_vel;
|
||||
} else {
|
||||
// no_diag false : the drone can translate on any possible direction
|
||||
if (fabs(yy) > y_dev_min)
|
||||
mvt->linear.y = yy * y_vel;
|
||||
if (fabs(xx) > x_dev_min)
|
||||
mvt->linear.x = - xx * x_vel;
|
||||
}
|
||||
} else // no_diag false : the drone can translate on any possible direction
|
||||
{
|
||||
if (fabs(yy) > y_dev_min)
|
||||
mvt->linear.y = yy * y_vel;
|
||||
if (fabs(xx) > x_dev_min)
|
||||
mvt->linear.x = - xx * x_vel;
|
||||
}
|
||||
|
||||
if (fabs(theta) > th_dev_min) {
|
||||
mvt->angular.z = theta * angle_vel;
|
||||
}
|
||||
if (fabs(theta) > th_dev_min) {
|
||||
mvt->angular.z = theta * angle_vel;
|
||||
}
|
||||
|
||||
pub.publish(mvt);
|
||||
ROS_INFO("cmd published");
|
||||
pub.publish(mvt);
|
||||
ROS_INFO("cmd published");
|
||||
}
|
||||
|
||||
public:
|
||||
Run(const ros::Publisher& cmd_publisher) :
|
||||
pub(cmd_publisher) {}
|
||||
public:
|
||||
explicit Run(const ros::Publisher& cmd_publisher) :
|
||||
pub(cmd_publisher) {}
|
||||
|
||||
void callback(const hand_control::Plan::ConstPtr& msg)
|
||||
// handle received messages
|
||||
{
|
||||
ROS_INFO("plan received");
|
||||
if (msg->curvature < max_curv && msg->number > min_number)
|
||||
// we ever have msg->curvature == 0 in fact (not implemented yet)
|
||||
{
|
||||
void callback(const hand_control::Plan::ConstPtr& msg) {
|
||||
ROS_INFO("plan received");
|
||||
|
||||
if(msg->normal.z > 0)
|
||||
{
|
||||
yy = msg->normal.x;
|
||||
xx = msg->normal.y;
|
||||
// we ever have msg->curvature == 0 in fact (not implemented yet)
|
||||
if (msg->curvature < max_curv && msg->number > min_number) {
|
||||
if (msg->normal.z > 0) {
|
||||
yy = msg->normal.x;
|
||||
xx = msg->normal.y;
|
||||
} else {
|
||||
yy = - msg->normal.x;
|
||||
xx = - msg->normal.y;
|
||||
}
|
||||
zz = msg->altitude - neutral_z;
|
||||
theta = msg->angle;
|
||||
// theta between -90 and 90
|
||||
ROS_INFO("coords updated");
|
||||
} else {
|
||||
xx = yy = zz = 0.;
|
||||
}
|
||||
else
|
||||
{
|
||||
yy = - msg->normal.x;
|
||||
xx = - msg->normal.y;
|
||||
}
|
||||
|
||||
zz = msg->altitude - neutral_z;
|
||||
|
||||
theta = msg->angle;
|
||||
// theta between -90 and 90
|
||||
|
||||
ROS_INFO("coords updated");
|
||||
} else {
|
||||
xx = yy = zz = 0.;
|
||||
}
|
||||
publish();
|
||||
};
|
||||
|
||||
void reconfigure(const hand_control::CommanderConfig& c, const uint32_t& level)
|
||||
// updates the parameters (received with dynamic_reconfigure)
|
||||
{
|
||||
max_curv = c.max_curvature;
|
||||
x_dev_min = c.x_minimal_deviation;
|
||||
y_dev_min = c.y_minimal_deviation;
|
||||
z_dev_min = c.z_minimal_deviation;
|
||||
th_dev_min = c.theta_minimal_deviation;
|
||||
neutral_z = c.neutral_alt;
|
||||
min_number = c.min_points_number;
|
||||
up_factor = c.up_fact;
|
||||
x_vel = c.x_vel;
|
||||
y_vel = c.y_vel;
|
||||
z_vel = c.z_vel;
|
||||
angle_vel = c.angle_vel;
|
||||
no_diag = c.no_diag;
|
||||
}
|
||||
|
||||
void run()
|
||||
// runs the callbacks and publications process
|
||||
{
|
||||
ros::spin();
|
||||
publish();
|
||||
}
|
||||
|
||||
// updates the parameters (received with dynamic_reconfigure)
|
||||
void reconfigure(const hand_control::CommanderConfig& c,
|
||||
const uint32_t& level) {
|
||||
max_curv = c.max_curvature;
|
||||
x_dev_min = c.x_minimal_deviation;
|
||||
y_dev_min = c.y_minimal_deviation;
|
||||
z_dev_min = c.z_minimal_deviation;
|
||||
th_dev_min = c.theta_minimal_deviation;
|
||||
neutral_z = c.neutral_alt;
|
||||
min_number = c.min_points_number;
|
||||
up_factor = c.up_fact;
|
||||
x_vel = c.x_vel;
|
||||
y_vel = c.y_vel;
|
||||
z_vel = c.z_vel;
|
||||
angle_vel = c.angle_vel;
|
||||
no_diag = c.no_diag;
|
||||
}
|
||||
|
||||
// runs the callbacks and publications process
|
||||
void run() {
|
||||
ros::spin();
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "commander");
|
||||
ros::NodeHandle node("commander");
|
||||
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
|
||||
Run run(cmd_pub);
|
||||
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "commander");
|
||||
ros::NodeHandle node("commander");
|
||||
ros::Publisher cmd_pub =\
|
||||
node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
|
||||
Run run(cmd_pub);
|
||||
ros::Subscriber plan_sub =\
|
||||
node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
|
||||
|
||||
// setting up dynamic_reconfigure (for rqt_reconfigure)
|
||||
dynamic_reconfigure::Server<hand_control::CommanderConfig> server;
|
||||
dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f;
|
||||
f = boost::bind(&Run::reconfigure, &run, _1, _2);
|
||||
server.setCallback(f);
|
||||
// setting up dynamic_reconfigure (for rqt_reconfigure)
|
||||
dynamic_reconfigure::Server<hand_control::CommanderConfig> server;
|
||||
dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f;
|
||||
f = boost::bind(&Run::reconfigure, &run, _1, _2);
|
||||
server.setCallback(f);
|
||||
|
||||
// starts working
|
||||
run.run();
|
||||
return 0;
|
||||
// starts working
|
||||
run.run();
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
198
src/display.cpp
198
src/display.cpp
|
|
@ -17,10 +17,12 @@
|
|||
*/
|
||||
|
||||
// library used by "keyboard_cmd.cpp"
|
||||
|
||||
#include "./display.h"
|
||||
|
||||
#include <ncurses.h>
|
||||
#include <string>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include "display.h"
|
||||
#include <string>
|
||||
|
||||
const int Curses::cmd_kbd_lines = 8;
|
||||
const int Curses::cmd_kbd_columns = 55;
|
||||
|
|
@ -41,110 +43,110 @@ const int Curses::topic_lines = 8;
|
|||
const int Curses::topic_columns = 22;
|
||||
|
||||
Curses::Curses() {
|
||||
initscr();
|
||||
cbreak();
|
||||
start_color();
|
||||
//noecho();
|
||||
initscr();
|
||||
cbreak();
|
||||
start_color();
|
||||
// noecho();
|
||||
|
||||
cmd_kbd = newwin(cmd_kbd_lines, cmd_kbd_columns, 0, 0);
|
||||
cmd_kbd = newwin(cmd_kbd_lines, cmd_kbd_columns, 0, 0);
|
||||
|
||||
get = newwin(get_lines, get_columns,
|
||||
get = newwin(get_lines, get_columns,
|
||||
cmd_kbd_lines, cmd_kbd_columns/2);
|
||||
|
||||
cmd_speed = newwin(cmd_speed_lines, cmd_speed_columns,
|
||||
cmd_speed = newwin(cmd_speed_lines, cmd_speed_columns,
|
||||
cmd_kbd_lines + get_lines, 0);
|
||||
|
||||
log_sent_title = newwin(1, log_sent_w_columns,
|
||||
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,
|
||||
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);
|
||||
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,
|
||||
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);
|
||||
init_pair(2, COLOR_RED, COLOR_BLACK);
|
||||
wattron(nav_data, COLOR_PAIR(2));
|
||||
wattron(nav_data, A_BOLD);
|
||||
|
||||
topic = newwin(topic_lines, topic_columns,
|
||||
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);
|
||||
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();
|
||||
print_nav_data();
|
||||
print_cmd_kbd();
|
||||
print_cmd_speed();
|
||||
print_topic();
|
||||
|
||||
wmove(get, 0, 0);
|
||||
wrefresh(get);
|
||||
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();
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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:
|
||||
;
|
||||
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);
|
||||
}
|
||||
|
|
@ -170,39 +172,39 @@ void Curses::update_nav_data(const float& batteryPercent,
|
|||
wmove(nav_data, 2, 10);
|
||||
wprintw(nav_data, "%f %", time);
|
||||
wmove(nav_data, 1, 10);
|
||||
switch(state) {
|
||||
switch (state) {
|
||||
case 0:
|
||||
waddstr(nav_data, "unknown ");
|
||||
break;
|
||||
waddstr(nav_data, "unknown ");
|
||||
break;
|
||||
case 1:
|
||||
waddstr(nav_data, "inited ");
|
||||
break;
|
||||
waddstr(nav_data, "inited ");
|
||||
break;
|
||||
case 2:
|
||||
waddstr(nav_data, "landed ");
|
||||
break;
|
||||
waddstr(nav_data, "landed ");
|
||||
break;
|
||||
case 3:
|
||||
waddstr(nav_data, "flying ");
|
||||
break;
|
||||
waddstr(nav_data, "flying ");
|
||||
break;
|
||||
case 4:
|
||||
waddstr(nav_data, "hovering ");
|
||||
break;
|
||||
waddstr(nav_data, "hovering ");
|
||||
break;
|
||||
case 5:
|
||||
waddstr(nav_data, "test ");
|
||||
break;
|
||||
waddstr(nav_data, "test ");
|
||||
break;
|
||||
case 6:
|
||||
waddstr(nav_data, "taking off ");
|
||||
break;
|
||||
waddstr(nav_data, "taking off ");
|
||||
break;
|
||||
case 7:
|
||||
waddstr(nav_data, "flying ");
|
||||
break;
|
||||
waddstr(nav_data, "flying ");
|
||||
break;
|
||||
case 8:
|
||||
waddstr(nav_data, "landing ");
|
||||
break;
|
||||
waddstr(nav_data, "landing ");
|
||||
break;
|
||||
case 9:
|
||||
waddstr(nav_data, "looping ");
|
||||
break;
|
||||
waddstr(nav_data, "looping ");
|
||||
break;
|
||||
default:
|
||||
;
|
||||
{}
|
||||
}
|
||||
wrefresh(nav_data);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -16,16 +16,15 @@
|
|||
* along with Hand Control. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef CURSES_DISPLAY
|
||||
#define CURSES_DISPLAY
|
||||
#ifndef SRC_DISPLAY_H_
|
||||
#define SRC_DISPLAY_H_
|
||||
|
||||
#include <ncurses.h>
|
||||
#include <string>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <string>
|
||||
|
||||
class Curses
|
||||
{
|
||||
private:
|
||||
class Curses {
|
||||
private:
|
||||
static const int cmd_kbd_lines;
|
||||
static const int cmd_kbd_columns;
|
||||
WINDOW* cmd_kbd;
|
||||
|
|
@ -50,18 +49,17 @@ class Curses
|
|||
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:
|
||||
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,
|
||||
|
|
@ -70,4 +68,4 @@ class Curses
|
|||
void update_topic(const geometry_msgs::Twist::ConstPtr& twist);
|
||||
};
|
||||
|
||||
#endif
|
||||
#endif // SRC_DISPLAY_H_
|
||||
|
|
|
|||
|
|
@ -1,17 +1,21 @@
|
|||
/* Copyright © 2015 CentraleSupélec
|
||||
*
|
||||
*
|
||||
|
||||
* This file is part of Hand Control.
|
||||
*
|
||||
*
|
||||
|
||||
* Hand Control is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
*
|
||||
|
||||
* Hand Control is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
*
|
||||
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Hand Control. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
|
@ -33,84 +37,93 @@ typedef pcl::PointCloud<Point> PointCloud;
|
|||
typedef Eigen::Matrix3f& Matrix;
|
||||
|
||||
class Callback {
|
||||
public:
|
||||
void callback(const PointCloud::ConstPtr& msg)
|
||||
public:
|
||||
// handles received messages and publish the plan estimation
|
||||
{
|
||||
ROS_INFO("PointCloud received");
|
||||
void callback(const PointCloud::ConstPtr& msg) {
|
||||
ROS_INFO("PointCloud received");
|
||||
|
||||
if (msg->width > 3){
|
||||
// else, no plan can be estimated…
|
||||
if (msg->width > 3) {
|
||||
// else, no plan can be estimated…
|
||||
analyser.setInputCloud(msg);
|
||||
Matrix eg = analyser.getEigenVectors();
|
||||
|
||||
analyser.setInputCloud(msg);
|
||||
Matrix eg = analyser.getEigenVectors();
|
||||
// to build the "Plan" message to be published
|
||||
float x, y, z, th, h, c;
|
||||
x = y = z = th = h = c = 0.;
|
||||
|
||||
// to build the "Plan" message to be published
|
||||
float x, y, z, th, h, c;
|
||||
x = y = z = th = h = c = 0.;
|
||||
// v = eg_1 ^ eg_2 is the plan normal
|
||||
Eigen::Vector3f v = eg.col(0).cross(eg.col(1));
|
||||
v.normalize(); // to have norm(v) == 1
|
||||
|
||||
// v = eg_1 ^ eg_2 is the plan normal
|
||||
Eigen::Vector3f v = eg.col(0).cross(eg.col(1));
|
||||
v.normalize(); // to have norm(v) == 1
|
||||
// x, y and z are the coords of the plan normal
|
||||
if (!reverse) {
|
||||
x = v(0); y = v(1);
|
||||
} else {
|
||||
// if "x" and "y" axes are inverted
|
||||
// (reverse is a parameter to set with dynamic_reconfigure)
|
||||
x = v(1); y = v(0);
|
||||
}
|
||||
z = v(2);
|
||||
|
||||
// x, y and z are the coords of the plan normal
|
||||
if (!reverse)
|
||||
{
|
||||
x = v(0); y = v(1);
|
||||
} else {
|
||||
// if "x" and "y" axes are inverted
|
||||
// (reverse is a parameter to set with dynamic_reconfigure)
|
||||
x = v(1); y = v(0);
|
||||
// h is the altitude
|
||||
h = (analyser.getMean())(2);
|
||||
|
||||
|
||||
// angle calculation
|
||||
|
||||
|
||||
// m_x and m_y are the "x" and "y" coords
|
||||
// of the first principal component
|
||||
float m_x, m_y;
|
||||
|
||||
|
||||
|
||||
// parameter to set
|
||||
// with dynamic_reconfigure
|
||||
if (reverse_angle) {
|
||||
m_x = eg(0, 0);
|
||||
m_y = eg(1, 0);
|
||||
} else {
|
||||
m_x = eg(1, 0);
|
||||
m_y = eg(0, 0);
|
||||
}
|
||||
|
||||
// because we want "th" only between -90° and 90°
|
||||
if (m_x < 0.)
|
||||
m_y *= -1;
|
||||
|
||||
th = - asin(m_y / sqrt(pow(m_y, 2)+ pow(m_x, 2))); // 0 <= th <= pi
|
||||
th *= _RAD2DEG; // -90 <= th <= 90
|
||||
|
||||
// TODO(someone)
|
||||
// -> calculate "c" (the curvature)
|
||||
// ( c == 0 for the moment)
|
||||
|
||||
// publication
|
||||
ROS_INFO("Plan published");
|
||||
publisher.publish(
|
||||
to_Plan(
|
||||
x, y, z, h, th, c,
|
||||
msg->header.seq,
|
||||
msg->header.stamp,
|
||||
msg->width));
|
||||
}
|
||||
z = v(2);
|
||||
|
||||
// h is the altitude
|
||||
h = (analyser.getMean())(2);
|
||||
|
||||
// angle calculation
|
||||
|
||||
// m_x and m_y are the "x" and "y" coords
|
||||
// of the first principal component
|
||||
float m_x, m_y;
|
||||
|
||||
if (reverse_angle)
|
||||
// parameter to set
|
||||
// with dynamic_reconfigure
|
||||
{
|
||||
m_x = eg(0,0);
|
||||
m_y = eg(1,0);
|
||||
} else {
|
||||
m_x = eg(1,0);
|
||||
m_y = eg(0,0);
|
||||
}
|
||||
|
||||
// because we want "th" only between -90° and 90°
|
||||
if (m_x < 0.)
|
||||
m_y *= -1;
|
||||
|
||||
th = - asin(m_y / sqrt(pow(m_y,2)+ pow(m_x,2))); // 0 <= th <= pi
|
||||
th *= _RAD2DEG; // -90 <= th <= 90
|
||||
|
||||
// TODO
|
||||
// -> calculate "c" (the curvature)
|
||||
// ( c == 0 for the moment)
|
||||
|
||||
// publication
|
||||
ROS_INFO("Plan published");
|
||||
publisher.publish(to_Plan(x, y, z, h, th, c, msg->header.seq, msg->header.stamp, msg->width));
|
||||
}
|
||||
}
|
||||
|
||||
Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)), reverse(false), reverse_angle(false) {};
|
||||
explicit Callback(const ros::Publisher& pub) :
|
||||
publisher(pub),
|
||||
_RAD2DEG(45.f/atan(1.)),
|
||||
reverse(false),
|
||||
reverse_angle(false) {}
|
||||
|
||||
void reconfigure(const hand_control::EstimatorConfig& c, const uint32_t& level)
|
||||
// updates the parameters received from dynamic_reconfigure
|
||||
{
|
||||
reverse = c.reverse ;
|
||||
reverse_angle = c.reverse_angle;
|
||||
void reconfigure(const hand_control::EstimatorConfig& c,
|
||||
const uint32_t& level) {
|
||||
reverse = c.reverse;
|
||||
reverse_angle = c.reverse_angle;
|
||||
}
|
||||
|
||||
private:
|
||||
private:
|
||||
ros::Publisher publisher;
|
||||
pcl::PCA<Point> analyser;
|
||||
const float _RAD2DEG;
|
||||
|
|
@ -119,47 +132,48 @@ class Callback {
|
|||
// return a "Plan" message build with
|
||||
// the informations provided
|
||||
inline const hand_control::Plan::ConstPtr
|
||||
to_Plan(const float& x, const float& y,
|
||||
const float& z, const float& h,
|
||||
const float& th,
|
||||
const float& c, const uint32_t& seq,
|
||||
const uint64_t& msec64, const uint64_t& number)
|
||||
{
|
||||
hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
|
||||
ros_msg->normal.x = x;
|
||||
ros_msg->normal.y = y;
|
||||
ros_msg->normal.z = z;
|
||||
ros_msg->altitude = h;
|
||||
ros_msg->angle = th;
|
||||
ros_msg->curvature = c;
|
||||
ros_msg->number = number;
|
||||
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;
|
||||
}
|
||||
to_Plan(const float& x, const float& y,
|
||||
const float& z, const float& h,
|
||||
const float& th,
|
||||
const float& c, const uint32_t& seq,
|
||||
const uint64_t& msec64, const uint64_t& number) {
|
||||
hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
|
||||
ros_msg->normal.x = x;
|
||||
ros_msg->normal.y = y;
|
||||
ros_msg->normal.z = z;
|
||||
|
||||
ros_msg->altitude = h;
|
||||
ros_msg->angle = th;
|
||||
ros_msg->curvature = c;
|
||||
|
||||
ros_msg->number = number;
|
||||
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;
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "estimator");
|
||||
ros::NodeHandle node("estimator");
|
||||
ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1);
|
||||
Callback callback(publisher);
|
||||
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, &Callback::callback, &callback);
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "estimator");
|
||||
ros::NodeHandle node("estimator");
|
||||
ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1);
|
||||
Callback callback(publisher);
|
||||
ros::Subscriber subscriber =
|
||||
node.subscribe<PointCloud>("input", 1, &Callback::callback, &callback);
|
||||
|
||||
// sets up dynamic_reconfigure
|
||||
dynamic_reconfigure::Server<hand_control::EstimatorConfig> server;
|
||||
dynamic_reconfigure::Server<hand_control::EstimatorConfig>::CallbackType f;
|
||||
f = boost::bind(&Callback::reconfigure, &callback, _1, _2);
|
||||
server.setCallback(f);
|
||||
// sets up dynamic_reconfigure
|
||||
dynamic_reconfigure::Server<hand_control::EstimatorConfig> server;
|
||||
dynamic_reconfigure::Server<hand_control::EstimatorConfig>::CallbackType f;
|
||||
f = boost::bind(&Callback::reconfigure, &callback, _1, _2);
|
||||
server.setCallback(f);
|
||||
|
||||
// begins working
|
||||
ROS_INFO("node started");
|
||||
ros::spin();
|
||||
ROS_INFO("exit");
|
||||
return 0;
|
||||
// begins working
|
||||
ROS_INFO("node started");
|
||||
ros::spin();
|
||||
ROS_INFO("exit");
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
165
src/filter.cpp
165
src/filter.cpp
|
|
@ -16,109 +16,122 @@
|
|||
* along with Hand Control. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#include <assert.h>
|
||||
#include <hand_control/FilterConfig.h>
|
||||
#include <ros/ros.h>
|
||||
#include <pcl_ros/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/tracking/impl/hsv_color_coherence.hpp>
|
||||
#include <assert.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <hand_control/FilterConfig.h>
|
||||
#include <algorithm>
|
||||
|
||||
|
||||
typedef pcl::PointXYZRGB Point;
|
||||
typedef pcl::PointCloud<Point> PointCloud;
|
||||
|
||||
class Callback {
|
||||
public:
|
||||
void
|
||||
callback(const PointCloud::ConstPtr& msg)
|
||||
public:
|
||||
// handles and filters the received PointCloud and
|
||||
// publishes the filtered PointCloud
|
||||
{
|
||||
PointCloud::Ptr pcl(new PointCloud());
|
||||
copy_info(msg, pcl); // copy the header
|
||||
BOOST_FOREACH (const Point& pt, msg->points)
|
||||
{
|
||||
float hue_dist, sat, val;
|
||||
hdist_s_v(pt, hue_dist, sat, val);
|
||||
if (pt.z < z_max and hue_dist < delta_hue and sat < sat_max and sat > sat_min and val < val_max and val > val_min)
|
||||
pcl->push_back(pt);
|
||||
}
|
||||
pcl->height = 1;
|
||||
pcl->width = pcl->points.size();
|
||||
publisher.publish(pcl);
|
||||
void callback(const PointCloud::ConstPtr& msg) {
|
||||
PointCloud::Ptr pcl(new PointCloud());
|
||||
copy_info(msg, pcl); // copy the header
|
||||
BOOST_FOREACH(const Point& pt, msg->points) {
|
||||
float hue_dist, sat, val;
|
||||
hdist_s_v(pt, hue_dist, sat, val);
|
||||
if (pt.z < z_max &&
|
||||
hue_dist < delta_hue &&
|
||||
sat < sat_max &&
|
||||
sat > sat_min &&
|
||||
val < val_max &&
|
||||
val > val_min) {
|
||||
pcl->push_back(pt);
|
||||
}
|
||||
}
|
||||
pcl->height = 1;
|
||||
pcl->width = pcl->points.size();
|
||||
publisher.publish(pcl);
|
||||
}
|
||||
|
||||
Callback(const ros::Publisher& pub)
|
||||
: publisher(pub), z_max(90.), hue(0.), delta_hue(20.), sat_min(0.3), sat_max(1.), val_min(0.3), val_max(1.)
|
||||
{}
|
||||
explicit Callback(const ros::Publisher& pub) :
|
||||
publisher(pub),
|
||||
z_max(90.),
|
||||
hue(0.),
|
||||
delta_hue(20.),
|
||||
sat_min(0.3),
|
||||
sat_max(1.),
|
||||
val_min(0.3),
|
||||
val_max(1.) {}
|
||||
|
||||
void
|
||||
reconfigure(const hand_control::FilterConfig& c, const uint32_t& level)
|
||||
// updates the parameters
|
||||
{
|
||||
z_max = c.z_max;
|
||||
hue = c.hue;
|
||||
delta_hue = c.delta_hue;
|
||||
val_min = c.val_min;
|
||||
val_max = c.val_max;
|
||||
sat_min = c.sat_min;
|
||||
sat_max = c.sat_max;
|
||||
void reconfigure(const hand_control::FilterConfig& c,
|
||||
const uint32_t& level) {
|
||||
z_max = c.z_max;
|
||||
hue = c.hue;
|
||||
delta_hue = c.delta_hue;
|
||||
val_min = c.val_min;
|
||||
val_max = c.val_max;
|
||||
sat_min = c.sat_min;
|
||||
sat_max = c.sat_max;
|
||||
}
|
||||
|
||||
private:
|
||||
private:
|
||||
ros::Publisher publisher;
|
||||
float z_max, hue, delta_hue, val_min, val_max, sat_min, sat_max;
|
||||
float z_max, hue, delta_hue, val_min,
|
||||
val_max, sat_min, sat_max;
|
||||
|
||||
inline
|
||||
void
|
||||
copy_info(const PointCloud::ConstPtr& a,
|
||||
PointCloud::Ptr& b)
|
||||
// copy the header info (useful in order to use rviz)
|
||||
{
|
||||
b->header = a->header;
|
||||
b->sensor_origin_ = a->sensor_origin_;
|
||||
b->sensor_orientation_ = a->sensor_orientation_;
|
||||
b->is_dense = a->is_dense;
|
||||
inline void copy_info(const PointCloud::ConstPtr& a,
|
||||
PointCloud::Ptr& b) {
|
||||
b->header = a->header;
|
||||
b->sensor_origin_ = a->sensor_origin_;
|
||||
b->sensor_orientation_ = a->sensor_orientation_;
|
||||
b->is_dense = a->is_dense;
|
||||
}
|
||||
|
||||
inline
|
||||
void
|
||||
hdist_s_v(const Point& pt, float& h_dist, float& s, float& v)
|
||||
// calculate the distance from the wished hue, the saturation and the value
|
||||
// of the point
|
||||
{
|
||||
float h, diff1, diff2;
|
||||
pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
|
||||
h *= 360.0f ;
|
||||
diff1 = std::fabs(h - hue);
|
||||
// hue is periodic
|
||||
if (h < hue)
|
||||
diff2 = std::fabs(360.0f + h - hue);
|
||||
else
|
||||
diff2 = std::fabs(360.0f + hue - h);
|
||||
h_dist = std::min(diff1, diff2);
|
||||
// calculate the distance from the wished hue,
|
||||
// the saturation and the value of the point
|
||||
inline void hdist_s_v(const Point& pt,
|
||||
float& h_dist,
|
||||
float& s,
|
||||
float& v) {
|
||||
float h, diff1, diff2;
|
||||
pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
|
||||
h *= 360.0f;
|
||||
diff1 = std::fabs(h - hue);
|
||||
// hue is periodic
|
||||
if (h < hue)
|
||||
diff2 = std::fabs(360.0f + h - hue);
|
||||
else
|
||||
diff2 = std::fabs(360.0f + hue - h);
|
||||
h_dist = std::min(diff1, diff2);
|
||||
}
|
||||
};
|
||||
|
||||
int
|
||||
main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "filter");
|
||||
ros::NodeHandle node("filter");
|
||||
ros::Publisher publisher = node.advertise<PointCloud>("output", 1);
|
||||
Callback my_callback(publisher);
|
||||
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, &Callback::callback, &my_callback);
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "filter");
|
||||
ros::NodeHandle node("filter");
|
||||
|
||||
// sets up dynamic_reconfigure
|
||||
dynamic_reconfigure::Server<hand_control::FilterConfig> server;
|
||||
dynamic_reconfigure::Server<hand_control::FilterConfig>::CallbackType f;
|
||||
f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2);
|
||||
server.setCallback(f);
|
||||
ros::Publisher publisher =
|
||||
node.advertise<PointCloud>("output", 1);
|
||||
|
||||
// begins working
|
||||
ROS_INFO("node started");
|
||||
ros::spin();
|
||||
ROS_INFO("exit");
|
||||
return 0;
|
||||
Callback my_callback(publisher);
|
||||
|
||||
ros::Subscriber subscriber =
|
||||
node.subscribe<PointCloud>("input", 1,
|
||||
&Callback::callback,
|
||||
&my_callback);
|
||||
|
||||
// sets up dynamic_reconfigure
|
||||
dynamic_reconfigure::Server<hand_control::FilterConfig> server;
|
||||
dynamic_reconfigure::Server<hand_control::FilterConfig>::CallbackType f;
|
||||
f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2);
|
||||
server.setCallback(f);
|
||||
|
||||
// begins working
|
||||
ROS_INFO("node started");
|
||||
ros::spin();
|
||||
ROS_INFO("exit");
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -16,280 +16,254 @@
|
|||
* along with Hand Control. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <ros/time.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:
|
||||
#include "./display.h"
|
||||
|
||||
class NavdataCallback {
|
||||
private:
|
||||
boost::shared_ptr<Curses> term;
|
||||
|
||||
public:
|
||||
NavdataCallback(const boost::shared_ptr<Curses>& terminal) :
|
||||
term(terminal) {}
|
||||
public:
|
||||
explicit 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);
|
||||
term->update_nav_data(msg->batteryPercent, msg->state, msg->tm);
|
||||
}
|
||||
}; // class NavdataCallback
|
||||
};
|
||||
|
||||
class CmdVelCallback
|
||||
{
|
||||
private:
|
||||
class CmdVelCallback {
|
||||
private:
|
||||
boost::shared_ptr<Curses> term;
|
||||
|
||||
public:
|
||||
CmdVelCallback(const boost::shared_ptr<Curses>& terminal) :
|
||||
public:
|
||||
explicit 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
|
||||
{
|
||||
private:
|
||||
class Run {
|
||||
private:
|
||||
std_msgs::Empty empty;
|
||||
ros::NodeHandle n;
|
||||
ros::Rate loop_rate;
|
||||
ros::Publisher cmd, pub_takeoff, pub_land, pub_reset;
|
||||
ros::Subscriber data_subscriber, cmdvel_subscriber;
|
||||
void land() { pub_land.publish(empty); };
|
||||
void takeoff() { pub_takeoff.publish(empty); };
|
||||
void reset() { pub_reset.publish(empty); };
|
||||
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;
|
||||
boost::shared_ptr<Curses> term;
|
||||
NavdataCallback data_callback;
|
||||
CmdVelCallback cmdvel_callback;
|
||||
|
||||
public:
|
||||
Run(const boost::shared_ptr<Curses>& terminal) :
|
||||
data_callback(terminal),
|
||||
cmdvel_callback(terminal),
|
||||
term(terminal),
|
||||
loop_rate(30),
|
||||
x_speed(0.1),
|
||||
y_speed(0.1),
|
||||
z_speed(0.1),
|
||||
turn(0.1) {
|
||||
cmd = n.advertise<geometry_msgs::Twist>("/cmd_vel",1);
|
||||
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);
|
||||
public:
|
||||
explicit Run(const boost::shared_ptr<Curses>& terminal) :
|
||||
data_callback(terminal),
|
||||
cmdvel_callback(terminal),
|
||||
term(terminal),
|
||||
loop_rate(30),
|
||||
x_speed(0.1),
|
||||
y_speed(0.1),
|
||||
z_speed(0.1),
|
||||
turn(0.1) {
|
||||
cmd = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
|
||||
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);
|
||||
|
||||
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);
|
||||
data_subscriber =
|
||||
n.subscribe<ardrone_autonomy::Navdata>(
|
||||
"/ardrone/navdata", 1, data_callback);
|
||||
|
||||
float a(0);
|
||||
int s(0);
|
||||
float time(0);
|
||||
term->update_nav_data(a, s, time);
|
||||
}
|
||||
cmdvel_subscriber =
|
||||
n.subscribe<geometry_msgs::Twist>(
|
||||
"/cmd_vel", 1, cmdvel_callback);
|
||||
|
||||
void operator()()
|
||||
{
|
||||
while (ros::ok())
|
||||
{
|
||||
ros::spinOnce();
|
||||
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);
|
||||
|
||||
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.;
|
||||
float a(0);
|
||||
int s(0);
|
||||
float time(0);
|
||||
term->update_nav_data(a, s, time);
|
||||
}
|
||||
|
||||
char c = term->getchar();
|
||||
void operator()() {
|
||||
while (ros::ok()) {
|
||||
ros::spinOnce();
|
||||
|
||||
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' :
|
||||
{//turn left
|
||||
msg->angular.z = turn;
|
||||
msg->linear.x = x_speed;
|
||||
cmd.publish(msg);
|
||||
term->log_sent("forward left !");
|
||||
break;
|
||||
}
|
||||
case 'o' :
|
||||
{//turn right
|
||||
msg->angular.z = -turn;
|
||||
msg->linear.x = x_speed;
|
||||
cmd.publish(msg);
|
||||
term->log_sent("forward right !");
|
||||
break;
|
||||
}
|
||||
case ',' :
|
||||
{//turn left backward
|
||||
msg->angular.z = turn;
|
||||
msg->linear.x = -x_speed;
|
||||
cmd.publish(msg);
|
||||
term->log_sent("backward left !");
|
||||
break;
|
||||
}
|
||||
case ':' :
|
||||
{//turn right backward
|
||||
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;
|
||||
term->update_cmd_speed('x', x_speed);
|
||||
break;
|
||||
}
|
||||
case 'w' :
|
||||
{// - x_speed
|
||||
x_speed *= 0.9;
|
||||
term->update_cmd_speed('x', x_speed);
|
||||
break;
|
||||
}
|
||||
case 'z' :
|
||||
{// + y_speed
|
||||
y_speed *= 1.1;
|
||||
term->update_cmd_speed('y', y_speed);
|
||||
break;
|
||||
}
|
||||
case 'x' :
|
||||
{// - y_speed
|
||||
y_speed *= 0.9;
|
||||
term->update_cmd_speed('y', y_speed);
|
||||
break;
|
||||
}
|
||||
case 'e' :
|
||||
{// + z_speed
|
||||
z_speed *= 1.1;
|
||||
term->update_cmd_speed('z', z_speed);
|
||||
break;
|
||||
}
|
||||
case 'c' :
|
||||
{// - z_speed
|
||||
z_speed *= 0.9;
|
||||
term->update_cmd_speed('z', z_speed);
|
||||
break;
|
||||
}
|
||||
case 'r' :
|
||||
{// + turn speed
|
||||
turn *= 1.1;
|
||||
term->update_cmd_speed('t', turn);
|
||||
break;
|
||||
}
|
||||
case 'v' :
|
||||
{// - turn speed
|
||||
turn *= 0.9;
|
||||
term->update_cmd_speed('t', turn);
|
||||
break;
|
||||
}
|
||||
default :
|
||||
{
|
||||
cmd.publish(msg);
|
||||
term->log_sent("hover !");
|
||||
}
|
||||
} // switch(c)
|
||||
loop_rate.sleep();
|
||||
} // while
|
||||
} //void run()
|
||||
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.;
|
||||
|
||||
}; // class Run
|
||||
char c = term->getchar();
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
setlocale(LC_ALL, "");
|
||||
ros::init(argc, argv, "keyboard_cmd");
|
||||
boost::shared_ptr<Curses> term(new Curses());
|
||||
Run fun(term);
|
||||
fun();
|
||||
return 0;
|
||||
} // main
|
||||
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' : { // turn left
|
||||
msg->angular.z = turn;
|
||||
msg->linear.x = x_speed;
|
||||
cmd.publish(msg);
|
||||
term->log_sent("forward left !");
|
||||
break;
|
||||
}
|
||||
case 'o' : { // turn right
|
||||
msg->angular.z = -turn;
|
||||
msg->linear.x = x_speed;
|
||||
cmd.publish(msg);
|
||||
term->log_sent("forward right !");
|
||||
break;
|
||||
}
|
||||
case ',' : { // turn left backward
|
||||
msg->angular.z = turn;
|
||||
msg->linear.x = -x_speed;
|
||||
cmd.publish(msg);
|
||||
term->log_sent("backward left !");
|
||||
break;
|
||||
}
|
||||
case ':' : { // turn right backward
|
||||
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;
|
||||
term->update_cmd_speed('x', x_speed);
|
||||
break;
|
||||
}
|
||||
case 'w' : { // - x_speed
|
||||
x_speed *= 0.9;
|
||||
term->update_cmd_speed('x', x_speed);
|
||||
break;
|
||||
}
|
||||
case 'z' : { // + y_speed
|
||||
y_speed *= 1.1;
|
||||
term->update_cmd_speed('y', y_speed);
|
||||
break;
|
||||
}
|
||||
case 'x' : { // - y_speed
|
||||
y_speed *= 0.9;
|
||||
term->update_cmd_speed('y', y_speed);
|
||||
break;
|
||||
}
|
||||
case 'e' : { // + z_speed
|
||||
z_speed *= 1.1;
|
||||
term->update_cmd_speed('z', z_speed);
|
||||
break;
|
||||
}
|
||||
case 'c' : { // - z_speed
|
||||
z_speed *= 0.9;
|
||||
term->update_cmd_speed('z', z_speed);
|
||||
break;
|
||||
}
|
||||
case 'r' : { // + turn speed
|
||||
turn *= 1.1;
|
||||
term->update_cmd_speed('t', turn);
|
||||
break;
|
||||
}
|
||||
case 'v' : { // - turn speed
|
||||
turn *= 0.9;
|
||||
term->update_cmd_speed('t', turn);
|
||||
break;
|
||||
}
|
||||
default : {
|
||||
cmd.publish(msg);
|
||||
term->log_sent("hover !");
|
||||
}
|
||||
} // switch(c)
|
||||
loop_rate.sleep();
|
||||
} // while
|
||||
} // void run()
|
||||
}; // class Run
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
setlocale(LC_ALL, "");
|
||||
ros::init(argc, argv, "keyboard_cmd");
|
||||
boost::shared_ptr<Curses> term(new Curses());
|
||||
Run fun(term);
|
||||
fun();
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue