diff --git a/src/commander.cpp b/src/commander.cpp
index 5750b63..f4fedda 100644
--- a/src/commander.cpp
+++ b/src/commander.cpp
@@ -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 .
+ * along with Hand Control. If not, see .
*/
+#include
+#include
+#include
+
#include
#include
-#include
-#include
-#include
-#include
#include
#include
#include
#include
-#include
#include
#include
+#include
-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("/cmd_vel", 1);
- Run run(cmd_pub);
- ros::Subscriber plan_sub = node.subscribe("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("/cmd_vel", 1);
+ Run run(cmd_pub);
+ ros::Subscriber plan_sub =\
+ node.subscribe("input", 1, &Run::callback, &run);
- // setting up dynamic_reconfigure (for rqt_reconfigure)
- dynamic_reconfigure::Server server;
- dynamic_reconfigure::Server::CallbackType f;
- f = boost::bind(&Run::reconfigure, &run, _1, _2);
- server.setCallback(f);
+ // setting up dynamic_reconfigure (for rqt_reconfigure)
+ dynamic_reconfigure::Server server;
+ dynamic_reconfigure::Server::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;
}
diff --git a/src/display.cpp b/src/display.cpp
index 4322599..2bc5bad 100644
--- a/src/display.cpp
+++ b/src/display.cpp
@@ -17,10 +17,12 @@
*/
// library used by "keyboard_cmd.cpp"
+
+#include "./display.h"
+
#include
-#include
#include
-#include "display.h"
+#include
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);
}
diff --git a/src/display.h b/src/display.h
index 5a0192b..14a58f5 100644
--- a/src/display.h
+++ b/src/display.h
@@ -16,16 +16,15 @@
* along with Hand Control. If not, see .
*/
-#ifndef CURSES_DISPLAY
-#define CURSES_DISPLAY
+#ifndef SRC_DISPLAY_H_
+#define SRC_DISPLAY_H_
#include
-#include
#include
+#include
-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_
diff --git a/src/estimator.cpp b/src/estimator.cpp
index 426f332..e1de141 100644
--- a/src/estimator.cpp
+++ b/src/estimator.cpp
@@ -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 .
*/
@@ -33,84 +37,93 @@ typedef pcl::PointCloud 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 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("output", 1);
- Callback callback(publisher);
- ros::Subscriber subscriber = node.subscribe("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("output", 1);
+ Callback callback(publisher);
+ ros::Subscriber subscriber =
+ node.subscribe("input", 1, &Callback::callback, &callback);
- // sets up dynamic_reconfigure
- dynamic_reconfigure::Server server;
- dynamic_reconfigure::Server::CallbackType f;
- f = boost::bind(&Callback::reconfigure, &callback, _1, _2);
- server.setCallback(f);
+ // sets up dynamic_reconfigure
+ dynamic_reconfigure::Server server;
+ dynamic_reconfigure::Server::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;
}
diff --git a/src/filter.cpp b/src/filter.cpp
index 03100ae..1a4a680 100644
--- a/src/filter.cpp
+++ b/src/filter.cpp
@@ -16,109 +16,122 @@
* along with Hand Control. If not, see .
*/
+
+#include
+#include
#include
#include
#include
#include
-#include
#include
-#include
+#include
typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud 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("output", 1);
- Callback my_callback(publisher);
- ros::Subscriber subscriber = node.subscribe("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 server;
- dynamic_reconfigure::Server::CallbackType f;
- f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2);
- server.setCallback(f);
+ ros::Publisher publisher =
+ node.advertise("output", 1);
- // begins working
- ROS_INFO("node started");
- ros::spin();
- ROS_INFO("exit");
- return 0;
+ Callback my_callback(publisher);
+
+ ros::Subscriber subscriber =
+ node.subscribe("input", 1,
+ &Callback::callback,
+ &my_callback);
+
+ // sets up dynamic_reconfigure
+ dynamic_reconfigure::Server server;
+ dynamic_reconfigure::Server::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;
}
diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp
index 0913c3d..280295b 100644
--- a/src/keyboard_cmd.cpp
+++ b/src/keyboard_cmd.cpp
@@ -16,280 +16,254 @@
* along with Hand Control. If not, see .
*/
+
#include
#include
#include
-#include "display.h"
#include
#include
#include
-class NavdataCallback
-{
- private:
+#include "./display.h"
+
+class NavdataCallback {
+ private:
boost::shared_ptr term;
- public:
- NavdataCallback(const boost::shared_ptr& terminal) :
- term(terminal) {}
+ public:
+ explicit NavdataCallback(const boost::shared_ptr& 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 term;
- public:
- CmdVelCallback(const boost::shared_ptr& terminal) :
+ public:
+ explicit CmdVelCallback(const boost::shared_ptr& 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 term;
NavdataCallback data_callback;
CmdVelCallback cmdvel_callback;
- public:
- Run(const boost::shared_ptr& 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("/cmd_vel",1);
- pub_takeoff = n.advertise("/ardrone/takeoff", 1);
- pub_land = n.advertise("/ardrone/land", 1);
- pub_reset = n.advertise("/ardrone/reset", 1);
- data_subscriber = n.subscribe("/ardrone/navdata", 1, data_callback);
- cmdvel_subscriber = n.subscribe("/cmd_vel", 1, cmdvel_callback);
+ public:
+ explicit Run(const boost::shared_ptr& 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("/cmd_vel", 1);
+ pub_takeoff = n.advertise("/ardrone/takeoff", 1);
+ pub_land = n.advertise("/ardrone/land", 1);
+ pub_reset = n.advertise("/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/navdata", 1, data_callback);
- float a(0);
- int s(0);
- float time(0);
- term->update_nav_data(a, s, time);
- }
+ cmdvel_subscriber =
+ n.subscribe(
+ "/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 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 term(new Curses());
+ Run fun(term);
+ fun();
+ return 0;
+}