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

complies with cpplint

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-09-19 18:54:10 +02:00
parent f94492264d
commit 15b11c3bb5
6 changed files with 635 additions and 648 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

226
src/commander.cpp
View file

@ -9,164 +9,150 @@
* *
* Hand Control is distributed in the hope that it will be useful, * Hand Control is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * 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. * GNU General Public License for more details.
* *
* You should have received a copy of the GNU General Public License * 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/ros.h>
#include <ros/time.h> #include <ros/time.h>
#include <locale.h>
#include <limits>
#include <math.h>
#include <assert.h>
#include <pcl_ros/point_cloud.h> #include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <hand_control/Plan.h> #include <hand_control/Plan.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <math.h>
#include <dynamic_reconfigure/server.h> #include <dynamic_reconfigure/server.h>
#include <hand_control/CommanderConfig.h> #include <hand_control/CommanderConfig.h>
#include <limits>
class Run
{ class Run {
private: private:
float xx, yy, zz, theta; // read coords float xx, yy, zz, theta; // read coords
// see README.md to know what are the parameters // see README.md to know what are the parameters
float x_vel, y_vel, z_vel, angle_vel, up_factor, neutral_z; // parameters float x_vel, y_vel, z_vel, angle_vel,
float max_curv; // not used yet up_factor, neutral_z; // parameters
float z_dev_min, x_dev_min, y_dev_min, th_dev_min; // parameters : thresholds float max_curv; // not used yet
uint64_t min_number; // parameter float z_dev_min, x_dev_min, y_dev_min,
bool no_diag; // parameter th_dev_min; // parameters : thresholds
uint64_t min_number; // parameter
bool no_diag; // parameter
ros::Publisher pub; ros::Publisher pub;
void publish() void publish() {
// build and publish a message from the "xx", "yy", "zz" and "theta" informations // build and publish a message from the "xx",
{ // "yy", "zz" and "theta" informations
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist()); geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
if (fabs(zz) > z_dev_min) if (fabs(zz) > z_dev_min) {
{ // up_factor to balance out the gravity effect
// up_factor to balance out the gravity effect if (zz > 0)
if (zz > 0) mvt->linear.z = zz * z_vel * up_factor;
mvt->linear.z = zz * z_vel * up_factor ; else
else mvt->linear.z = zz * z_vel;
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;
} }
else if (fabs(xx) > x_dev_min)
{ // no_diag true : the drone can only translate on the "x" axis
mvt->linear.x = - xx * x_vel; // 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) { if (fabs(theta) > th_dev_min) {
mvt->angular.z = theta * angle_vel; mvt->angular.z = theta * angle_vel;
} }
pub.publish(mvt); pub.publish(mvt);
ROS_INFO("cmd published"); ROS_INFO("cmd published");
} }
public: public:
Run(const ros::Publisher& cmd_publisher) : explicit Run(const ros::Publisher& cmd_publisher) :
pub(cmd_publisher) {} pub(cmd_publisher) {}
void callback(const hand_control::Plan::ConstPtr& msg)
// handle received messages // handle received messages
{ void callback(const hand_control::Plan::ConstPtr& msg) {
ROS_INFO("plan received"); ROS_INFO("plan received");
if (msg->curvature < max_curv && msg->number > min_number)
// we ever have msg->curvature == 0 in fact (not implemented yet)
{
if(msg->normal.z > 0) // we ever have msg->curvature == 0 in fact (not implemented yet)
{ if (msg->curvature < max_curv && msg->number > min_number) {
yy = msg->normal.x; if (msg->normal.z > 0) {
xx = msg->normal.y; 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 publish();
{
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();
} }
// 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) int main(int argc, char** argv) {
{ ros::init(argc, argv, "commander");
ros::init(argc, argv, "commander"); ros::NodeHandle node("commander");
ros::NodeHandle node("commander"); ros::Publisher cmd_pub =\
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1); node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
Run run(cmd_pub); Run run(cmd_pub);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run); ros::Subscriber plan_sub =\
node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
// setting up dynamic_reconfigure (for rqt_reconfigure) // setting up dynamic_reconfigure (for rqt_reconfigure)
dynamic_reconfigure::Server<hand_control::CommanderConfig> server; dynamic_reconfigure::Server<hand_control::CommanderConfig> server;
dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f; dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f;
f = boost::bind(&Run::reconfigure, &run, _1, _2); f = boost::bind(&Run::reconfigure, &run, _1, _2);
server.setCallback(f); server.setCallback(f);
// starts working // starts working
run.run(); run.run();
return 0; return 0;
} }

198
src/display.cpp
View file

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

18
src/display.h
View file

@ -16,16 +16,15 @@
* along with Hand Control. If not, see <http://www.gnu.org/licenses/>. * along with Hand Control. If not, see <http://www.gnu.org/licenses/>.
*/ */
#ifndef CURSES_DISPLAY #ifndef SRC_DISPLAY_H_
#define CURSES_DISPLAY #define SRC_DISPLAY_H_
#include <ncurses.h> #include <ncurses.h>
#include <string>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <string>
class Curses class Curses {
{ private:
private:
static const int cmd_kbd_lines; static const int cmd_kbd_lines;
static const int cmd_kbd_columns; static const int cmd_kbd_columns;
WINDOW* cmd_kbd; WINDOW* cmd_kbd;
@ -50,18 +49,17 @@ class Curses
static const int nav_data_columns; static const int nav_data_columns;
WINDOW* nav_data; WINDOW* nav_data;
void print_nav_data(); void print_nav_data();
static const int topic_lines; static const int topic_lines;
static const int topic_columns; static const int topic_columns;
WINDOW* topic; WINDOW* topic;
void print_topic(); void print_topic();
public: public:
Curses(); Curses();
~Curses(); ~Curses();
char getchar(); char getchar();
// TODO
void update_cmd_speed(const char& coord, const float& v); void update_cmd_speed(const char& coord, const float& v);
void update_nav_data(const float& batteryPercent, void update_nav_data(const float& batteryPercent,
const int& state, const int& state,
@ -70,4 +68,4 @@ class Curses
void update_topic(const geometry_msgs::Twist::ConstPtr& twist); void update_topic(const geometry_msgs::Twist::ConstPtr& twist);
}; };
#endif #endif // SRC_DISPLAY_H_

230
src/estimator.cpp
View file

@ -1,17 +1,21 @@
/* Copyright © 2015 CentraleSupélec /* Copyright © 2015 CentraleSupélec
* *
* This file is part of Hand Control. * This file is part of Hand Control.
* *
* Hand Control is free software: you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or * the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version. * (at your option) any later version.
* *
* Hand Control is distributed in the hope that it will be useful, * Hand Control is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * 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. * GNU General Public License for more details.
* *
* You should have received a copy of the GNU General Public License * 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/>.
*/ */
@ -33,84 +37,93 @@ typedef pcl::PointCloud<Point> PointCloud;
typedef Eigen::Matrix3f& Matrix; typedef Eigen::Matrix3f& Matrix;
class Callback { class Callback {
public: public:
void callback(const PointCloud::ConstPtr& msg)
// handles received messages and publish the plan estimation // handles received messages and publish the plan estimation
{ void callback(const PointCloud::ConstPtr& msg) {
ROS_INFO("PointCloud received"); ROS_INFO("PointCloud received");
if (msg->width > 3){ if (msg->width > 3) {
// else, no plan can be estimated… // else, no plan can be estimated…
analyser.setInputCloud(msg);
Matrix eg = analyser.getEigenVectors();
analyser.setInputCloud(msg); // to build the "Plan" message to be published
Matrix eg = analyser.getEigenVectors(); float x, y, z, th, h, c;
x = y = z = th = h = c = 0.;
// to build the "Plan" message to be published // v = eg_1 ^ eg_2 is the plan normal
float x, y, z, th, h, c; Eigen::Vector3f v = eg.col(0).cross(eg.col(1));
x = y = z = th = h = c = 0.; v.normalize(); // to have norm(v) == 1
// v = eg_1 ^ eg_2 is the plan normal // x, y and z are the coords of the plan normal
Eigen::Vector3f v = eg.col(0).cross(eg.col(1)); if (!reverse) {
v.normalize(); // to have norm(v) == 1 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 // h is the altitude
if (!reverse) h = (analyser.getMean())(2);
{
x = v(0); y = v(1);
} else { // angle calculation
// if "x" and "y" axes are inverted
// (reverse is a parameter to set with dynamic_reconfigure)
x = v(1); y = v(0); // 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 // updates the parameters received from dynamic_reconfigure
{ void reconfigure(const hand_control::EstimatorConfig& c,
reverse = c.reverse ; const uint32_t& level) {
reverse_angle = c.reverse_angle; reverse = c.reverse;
reverse_angle = c.reverse_angle;
} }
private: private:
ros::Publisher publisher; ros::Publisher publisher;
pcl::PCA<Point> analyser; pcl::PCA<Point> analyser;
const float _RAD2DEG; const float _RAD2DEG;
@ -119,47 +132,48 @@ class Callback {
// return a "Plan" message build with // return a "Plan" message build with
// the informations provided // the informations provided
inline const hand_control::Plan::ConstPtr inline const hand_control::Plan::ConstPtr
to_Plan(const float& x, const float& y, to_Plan(const float& x, const float& y,
const float& z, const float& h, const float& z, const float& h,
const float& th, const float& th,
const float& c, const uint32_t& seq, const float& c, const uint32_t& seq,
const uint64_t& msec64, const uint64_t& number) const uint64_t& msec64, const uint64_t& number) {
{ hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
hand_control::Plan::Ptr ros_msg(new hand_control::Plan()); ros_msg->normal.x = x;
ros_msg->normal.x = x; ros_msg->normal.y = y;
ros_msg->normal.y = y; ros_msg->normal.z = z;
ros_msg->normal.z = z;
ros_msg->altitude = h; ros_msg->altitude = h;
ros_msg->angle = th; ros_msg->angle = th;
ros_msg->curvature = c; ros_msg->curvature = c;
ros_msg->number = number;
uint64_t sec64 = msec64 / 1000000; ros_msg->number = number;
uint64_t nsec64 = (msec64 % 1000000) * 1000; uint64_t sec64 = msec64 / 1000000;
ros_msg->header.stamp.sec = (uint32_t) sec64; uint64_t nsec64 = (msec64 % 1000000) * 1000;
ros_msg->header.stamp.nsec = (uint32_t) nsec64; ros_msg->header.stamp.sec = (uint32_t) sec64;
ros_msg->header.seq = seq; ros_msg->header.stamp.nsec = (uint32_t) nsec64;
ros_msg->header.frame_id = "0"; ros_msg->header.seq = seq;
return ros_msg; ros_msg->header.frame_id = "0";
} return ros_msg;
}
}; };
int main(int argc, char** argv) int main(int argc, char** argv) {
{ ros::init(argc, argv, "estimator");
ros::init(argc, argv, "estimator"); ros::NodeHandle node("estimator");
ros::NodeHandle node("estimator"); ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1);
ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1); Callback callback(publisher);
Callback callback(publisher); ros::Subscriber subscriber =
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, &Callback::callback, &callback); node.subscribe<PointCloud>("input", 1, &Callback::callback, &callback);
// sets up dynamic_reconfigure // sets up dynamic_reconfigure
dynamic_reconfigure::Server<hand_control::EstimatorConfig> server; dynamic_reconfigure::Server<hand_control::EstimatorConfig> server;
dynamic_reconfigure::Server<hand_control::EstimatorConfig>::CallbackType f; dynamic_reconfigure::Server<hand_control::EstimatorConfig>::CallbackType f;
f = boost::bind(&Callback::reconfigure, &callback, _1, _2); f = boost::bind(&Callback::reconfigure, &callback, _1, _2);
server.setCallback(f); server.setCallback(f);
// begins working // begins working
ROS_INFO("node started"); ROS_INFO("node started");
ros::spin(); ros::spin();
ROS_INFO("exit"); ROS_INFO("exit");
return 0; return 0;
} }

165
src/filter.cpp
View file

@ -16,109 +16,122 @@
* 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 <assert.h>
#include <hand_control/FilterConfig.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <pcl_ros/point_cloud.h> #include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl/tracking/impl/hsv_color_coherence.hpp> #include <pcl/tracking/impl/hsv_color_coherence.hpp>
#include <assert.h>
#include <dynamic_reconfigure/server.h> #include <dynamic_reconfigure/server.h>
#include <hand_control/FilterConfig.h> #include <algorithm>
typedef pcl::PointXYZRGB Point; typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud; typedef pcl::PointCloud<Point> PointCloud;
class Callback { class Callback {
public: public:
void
callback(const PointCloud::ConstPtr& msg)
// handles and filters the received PointCloud and // handles and filters the received PointCloud and
// publishes the filtered PointCloud // publishes the filtered PointCloud
{ void callback(const PointCloud::ConstPtr& msg) {
PointCloud::Ptr pcl(new PointCloud()); PointCloud::Ptr pcl(new PointCloud());
copy_info(msg, pcl); // copy the header copy_info(msg, pcl); // copy the header
BOOST_FOREACH (const Point& pt, msg->points) BOOST_FOREACH(const Point& pt, msg->points) {
{ float hue_dist, sat, val;
float hue_dist, sat, val; hdist_s_v(pt, hue_dist, sat, val);
hdist_s_v(pt, hue_dist, sat, val); if (pt.z < z_max &&
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) hue_dist < delta_hue &&
pcl->push_back(pt); sat < sat_max &&
} sat > sat_min &&
pcl->height = 1; val < val_max &&
pcl->width = pcl->points.size(); val > val_min) {
publisher.publish(pcl); pcl->push_back(pt);
}
}
pcl->height = 1;
pcl->width = pcl->points.size();
publisher.publish(pcl);
} }
Callback(const ros::Publisher& pub) 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.) 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 // updates the parameters
{ void reconfigure(const hand_control::FilterConfig& c,
z_max = c.z_max; const uint32_t& level) {
hue = c.hue; z_max = c.z_max;
delta_hue = c.delta_hue; hue = c.hue;
val_min = c.val_min; delta_hue = c.delta_hue;
val_max = c.val_max; val_min = c.val_min;
sat_min = c.sat_min; val_max = c.val_max;
sat_max = c.sat_max; sat_min = c.sat_min;
sat_max = c.sat_max;
} }
private: private:
ros::Publisher publisher; 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) // copy the header info (useful in order to use rviz)
{ inline void copy_info(const PointCloud::ConstPtr& a,
b->header = a->header; PointCloud::Ptr& b) {
b->sensor_origin_ = a->sensor_origin_; b->header = a->header;
b->sensor_orientation_ = a->sensor_orientation_; b->sensor_origin_ = a->sensor_origin_;
b->is_dense = a->is_dense; b->sensor_orientation_ = a->sensor_orientation_;
b->is_dense = a->is_dense;
} }
inline // calculate the distance from the wished hue,
void // the saturation and the value of the point
hdist_s_v(const Point& pt, float& h_dist, float& s, float& v) inline void hdist_s_v(const Point& pt,
// calculate the distance from the wished hue, the saturation and the value float& h_dist,
// of the point float& s,
{ float& v) {
float h, diff1, diff2; float h, diff1, diff2;
pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v); pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
h *= 360.0f ; h *= 360.0f;
diff1 = std::fabs(h - hue); diff1 = std::fabs(h - hue);
// hue is periodic // hue is periodic
if (h < hue) if (h < hue)
diff2 = std::fabs(360.0f + h - hue); diff2 = std::fabs(360.0f + h - hue);
else else
diff2 = std::fabs(360.0f + hue - h); diff2 = std::fabs(360.0f + hue - h);
h_dist = std::min(diff1, diff2); h_dist = std::min(diff1, diff2);
} }
}; };
int int main(int argc, char** argv) {
main(int argc, char** argv) ros::init(argc, argv, "filter");
{ ros::NodeHandle node("filter");
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);
// sets up dynamic_reconfigure ros::Publisher publisher =
dynamic_reconfigure::Server<hand_control::FilterConfig> server; node.advertise<PointCloud>("output", 1);
dynamic_reconfigure::Server<hand_control::FilterConfig>::CallbackType f;
f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2);
server.setCallback(f);
// begins working Callback my_callback(publisher);
ROS_INFO("node started");
ros::spin(); ros::Subscriber subscriber =
ROS_INFO("exit"); node.subscribe<PointCloud>("input", 1,
return 0; &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;
} }

446
src/keyboard_cmd.cpp
View file

@ -16,280 +16,254 @@
* 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 <ros/ros.h> #include <ros/ros.h>
#include <ros/time.h> #include <ros/time.h>
#include <locale.h> #include <locale.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> #include <ardrone_autonomy/Navdata.h>
class NavdataCallback #include "./display.h"
{
private: class NavdataCallback {
private:
boost::shared_ptr<Curses> term; boost::shared_ptr<Curses> term;
public: public:
NavdataCallback(const boost::shared_ptr<Curses>& terminal) : explicit NavdataCallback(const boost::shared_ptr<Curses>& terminal) :
term(terminal) {} term(terminal) {}
void operator()(const ardrone_autonomy::Navdata::ConstPtr& msg) { void operator()(const ardrone_autonomy::Navdata::ConstPtr& msg) {
term->update_nav_data(msg->batteryPercent, msg->state, msg->tm); term->update_nav_data(msg->batteryPercent, msg->state, msg->tm);
} }
}; // class NavdataCallback };
class CmdVelCallback class CmdVelCallback {
{ private:
private:
boost::shared_ptr<Curses> term; boost::shared_ptr<Curses> term;
public: public:
CmdVelCallback(const boost::shared_ptr<Curses>& terminal) : explicit CmdVelCallback(const boost::shared_ptr<Curses>& terminal) :
term(terminal) {} term(terminal) {}
void operator()(const geometry_msgs::Twist::ConstPtr& msg) { void operator()(const geometry_msgs::Twist::ConstPtr& msg) {
term->update_topic(msg); term->update_topic(msg);
} }
}; // class CmdVelCallback };
class Run class Run {
{ private:
private:
std_msgs::Empty empty; std_msgs::Empty empty;
ros::NodeHandle n; ros::NodeHandle n;
ros::Rate loop_rate; ros::Rate loop_rate;
ros::Publisher cmd, pub_takeoff, pub_land, pub_reset; ros::Publisher cmd, pub_takeoff, pub_land, pub_reset;
ros::Subscriber data_subscriber, cmdvel_subscriber; ros::Subscriber data_subscriber, cmdvel_subscriber;
void land() { pub_land.publish(empty); }; void land() { pub_land.publish(empty); }
void takeoff() { pub_takeoff.publish(empty); }; void takeoff() { pub_takeoff.publish(empty); }
void reset() { pub_reset.publish(empty); }; void reset() { pub_reset.publish(empty); }
float x_speed, y_speed, z_speed, turn; float x_speed, y_speed, z_speed, turn;
boost::shared_ptr<Curses> term; boost::shared_ptr<Curses> term;
NavdataCallback data_callback; NavdataCallback data_callback;
CmdVelCallback cmdvel_callback; CmdVelCallback cmdvel_callback;
public: public:
Run(const boost::shared_ptr<Curses>& terminal) : explicit Run(const boost::shared_ptr<Curses>& terminal) :
data_callback(terminal), data_callback(terminal),
cmdvel_callback(terminal), cmdvel_callback(terminal),
term(terminal), term(terminal),
loop_rate(30), loop_rate(30),
x_speed(0.1), x_speed(0.1),
y_speed(0.1), y_speed(0.1),
z_speed(0.1), z_speed(0.1),
turn(0.1) { turn(0.1) {
cmd = n.advertise<geometry_msgs::Twist>("/cmd_vel",1); cmd = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
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);
term->update_cmd_speed('x', x_speed); data_subscriber =
term->update_cmd_speed('y', y_speed); n.subscribe<ardrone_autonomy::Navdata>(
term->update_cmd_speed('z', z_speed); "/ardrone/navdata", 1, data_callback);
term->update_cmd_speed('t', turn);
float a(0); cmdvel_subscriber =
int s(0); n.subscribe<geometry_msgs::Twist>(
float time(0); "/cmd_vel", 1, cmdvel_callback);
term->update_nav_data(a, s, time);
}
void operator()() term->update_cmd_speed('x', x_speed);
{ term->update_cmd_speed('y', y_speed);
while (ros::ok()) term->update_cmd_speed('z', z_speed);
{ term->update_cmd_speed('t', turn);
ros::spinOnce();
geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist()); float a(0);
msg->linear.x = msg->linear.y = msg->linear.z = int s(0);
msg->angular.x = msg->angular.y = msg->angular.z = 0.; float time(0);
term->update_nav_data(a, s, time);
}
char c = term->getchar(); void operator()() {
while (ros::ok()) {
ros::spinOnce();
switch(c) geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist());
{ msg->linear.x = msg->linear.y = msg->linear.z =
case 'k' : msg->angular.x = msg->angular.y = msg->angular.z = 0.;
{// 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 char c = term->getchar();
int main(int argc, char** argv) switch (c) {
{ case 'k' : { // hover
setlocale(LC_ALL, ""); cmd.publish(msg);
ros::init(argc, argv, "keyboard_cmd"); term->log_sent("hover !");
boost::shared_ptr<Curses> term(new Curses()); break;
Run fun(term); }
fun(); case 'i' : { // forward
return 0; msg->linear.x = x_speed;
} // main 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;
}