From 15b11c3bb53e0a9bc1fb29fff45ec018e12b3680 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 19 Sep 2015 18:54:10 +0200 Subject: [PATCH] complies with cpplint --- src/commander.cpp | 226 ++++++++++------------ src/display.cpp | 198 +++++++++---------- src/display.h | 18 +- src/estimator.cpp | 230 +++++++++++----------- src/filter.cpp | 165 ++++++++-------- src/keyboard_cmd.cpp | 446 ++++++++++++++++++++----------------------- 6 files changed, 635 insertions(+), 648 deletions(-) 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; +}