complies with cpplint

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

View file

@ -16,45 +16,46 @@
* 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/time.h>
#include <locale.h>
#include <limits>
#include <math.h> #include <math.h>
#include <assert.h> #include <assert.h>
#include <locale.h>
#include <ros/ros.h>
#include <ros/time.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,
up_factor, neutral_z; // parameters
float max_curv; // not used yet float max_curv; // not used yet
float z_dev_min, x_dev_min, y_dev_min, th_dev_min; // parameters : thresholds float z_dev_min, x_dev_min, y_dev_min,
th_dev_min; // parameters : thresholds
uint64_t min_number; // parameter uint64_t min_number; // parameter
bool no_diag; // 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;
@ -62,21 +63,15 @@ class Run
mvt->linear.z = zz * z_vel; mvt->linear.z = zz * z_vel;
} }
// no_diag true : the drone can only translate on the "x" axis // 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. // or the "y" axis but not on a linear combination of these axes.
if (no_diag) if (no_diag) {
{
if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min) if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
{
mvt->linear.y = yy * y_vel; mvt->linear.y = yy * y_vel;
}
else if (fabs(xx) > x_dev_min) else if (fabs(xx) > x_dev_min)
{
mvt->linear.x = - xx * x_vel; mvt->linear.x = - xx * x_vel;
} } else {
} else // no_diag false : the drone can translate on any possible direction // no_diag false : the drone can translate on any possible direction
{
if (fabs(yy) > y_dev_min) if (fabs(yy) > y_dev_min)
mvt->linear.y = yy * y_vel; mvt->linear.y = yy * y_vel;
if (fabs(xx) > x_dev_min) if (fabs(xx) > x_dev_min)
@ -92,43 +87,35 @@ class Run
} }
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) {
if (msg->normal.z > 0) {
yy = msg->normal.x; yy = msg->normal.x;
xx = msg->normal.y; xx = msg->normal.y;
} } else {
else
{
yy = - msg->normal.x; yy = - msg->normal.x;
xx = - msg->normal.y; xx = - msg->normal.y;
} }
zz = msg->altitude - neutral_z; zz = msg->altitude - neutral_z;
theta = msg->angle; theta = msg->angle;
// theta between -90 and 90 // theta between -90 and 90
ROS_INFO("coords updated"); ROS_INFO("coords updated");
} else { } else {
xx = yy = zz = 0.; xx = yy = zz = 0.;
} }
publish(); publish();
}; }
void reconfigure(const hand_control::CommanderConfig& c, const uint32_t& level)
// updates the parameters (received with dynamic_reconfigure) // updates the parameters (received with dynamic_reconfigure)
{ void reconfigure(const hand_control::CommanderConfig& c,
const uint32_t& level) {
max_curv = c.max_curvature; max_curv = c.max_curvature;
x_dev_min = c.x_minimal_deviation; x_dev_min = c.x_minimal_deviation;
y_dev_min = c.y_minimal_deviation; y_dev_min = c.y_minimal_deviation;
@ -144,21 +131,20 @@ class Run
no_diag = c.no_diag; no_diag = c.no_diag;
} }
void run()
// runs the callbacks and publications process // runs the callbacks and publications process
{ void run() {
ros::spin(); 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 = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1); ros::Publisher cmd_pub =\
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;

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;
@ -144,7 +146,7 @@ void Curses::update_cmd_speed(const char& coord, const float& v) {
wprintw(cmd_speed, "%f", v); wprintw(cmd_speed, "%f", v);
break; break;
default: default:
; {}
} }
wrefresh(cmd_speed); wrefresh(cmd_speed);
} }
@ -202,7 +204,7 @@ void Curses::update_nav_data(const float& batteryPercent,
waddstr(nav_data, "looping "); waddstr(nav_data, "looping ");
break; break;
default: default:
; {}
} }
wrefresh(nav_data); wrefresh(nav_data);
} }

View file

@ -16,15 +16,14 @@
* 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;
@ -61,7 +60,6 @@ class 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_

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/>.
*/ */
@ -34,14 +38,12 @@ 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); analyser.setInputCloud(msg);
Matrix eg = analyser.getEigenVectors(); Matrix eg = analyser.getEigenVectors();
@ -54,8 +56,7 @@ class Callback {
v.normalize(); // to have norm(v) == 1 v.normalize(); // to have norm(v) == 1
// x, y and z are the coords of the plan normal // x, y and z are the coords of the plan normal
if (!reverse) if (!reverse) {
{
x = v(0); y = v(1); x = v(0); y = v(1);
} else { } else {
// if "x" and "y" axes are inverted // if "x" and "y" axes are inverted
@ -67,16 +68,19 @@ class Callback {
// h is the altitude // h is the altitude
h = (analyser.getMean())(2); h = (analyser.getMean())(2);
// angle calculation // angle calculation
// m_x and m_y are the "x" and "y" coords // m_x and m_y are the "x" and "y" coords
// of the first principal component // of the first principal component
float m_x, m_y; float m_x, m_y;
if (reverse_angle)
// parameter to set // parameter to set
// with dynamic_reconfigure // with dynamic_reconfigure
{ if (reverse_angle) {
m_x = eg(0, 0); m_x = eg(0, 0);
m_y = eg(1, 0); m_y = eg(1, 0);
} else { } else {
@ -91,21 +95,30 @@ class Callback {
th = - asin(m_y / sqrt(pow(m_y, 2)+ pow(m_x, 2))); // 0 <= th <= pi th = - asin(m_y / sqrt(pow(m_y, 2)+ pow(m_x, 2))); // 0 <= th <= pi
th *= _RAD2DEG; // -90 <= th <= 90 th *= _RAD2DEG; // -90 <= th <= 90
// TODO // TODO(someone)
// -> calculate "c" (the curvature) // -> calculate "c" (the curvature)
// ( c == 0 for the moment) // ( c == 0 for the moment)
// publication // publication
ROS_INFO("Plan published"); ROS_INFO("Plan published");
publisher.publish(to_Plan(x, y, z, h, th, c, msg->header.seq, msg->header.stamp, msg->width)); 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,
const uint32_t& level) {
reverse = c.reverse; reverse = c.reverse;
reverse_angle = c.reverse_angle; reverse_angle = c.reverse_angle;
} }
@ -123,15 +136,16 @@ class Callback {
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; ros_msg->number = number;
uint64_t sec64 = msec64 / 1000000; uint64_t sec64 = msec64 / 1000000;
uint64_t nsec64 = (msec64 % 1000000) * 1000; uint64_t nsec64 = (msec64 % 1000000) * 1000;
@ -143,13 +157,13 @@ class Callback {
} }
}; };
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 = node.subscribe<PointCloud>("input", 1, &Callback::callback, &callback); ros::Subscriber subscriber =
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;

View file

@ -16,13 +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/>.
*/ */
#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;
@ -30,33 +32,41 @@ 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 and hue_dist < delta_hue and sat < sat_max and sat > sat_min and val < val_max and val > val_min) 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->push_back(pt);
} }
}
pcl->height = 1; pcl->height = 1;
pcl->width = pcl->points.size(); pcl->width = pcl->points.size();
publisher.publish(pcl); 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,
const uint32_t& level) {
z_max = c.z_max; z_max = c.z_max;
hue = c.hue; hue = c.hue;
delta_hue = c.delta_hue; delta_hue = c.delta_hue;
@ -68,26 +78,24 @@ class Callback {
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,
PointCloud::Ptr& b) {
b->header = a->header; b->header = a->header;
b->sensor_origin_ = a->sensor_origin_; b->sensor_origin_ = a->sensor_origin_;
b->sensor_orientation_ = a->sensor_orientation_; b->sensor_orientation_ = a->sensor_orientation_;
b->is_dense = a->is_dense; 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;
@ -101,14 +109,19 @@ class Callback {
} }
}; };
int int main(int argc, char** argv) {
main(int argc, char** argv)
{
ros::init(argc, argv, "filter"); ros::init(argc, argv, "filter");
ros::NodeHandle node("filter"); ros::NodeHandle node("filter");
ros::Publisher publisher = node.advertise<PointCloud>("output", 1);
ros::Publisher publisher =
node.advertise<PointCloud>("output", 1);
Callback my_callback(publisher); Callback my_callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, &Callback::callback, &my_callback);
ros::Subscriber subscriber =
node.subscribe<PointCloud>("input", 1,
&Callback::callback,
&my_callback);
// sets up dynamic_reconfigure // sets up dynamic_reconfigure
dynamic_reconfigure::Server<hand_control::FilterConfig> server; dynamic_reconfigure::Server<hand_control::FilterConfig> server;

View file

@ -16,61 +16,60 @@
* 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"
{
class NavdataCallback {
private: 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),
@ -83,8 +82,14 @@ class Run
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); 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); term->update_cmd_speed('x', x_speed);
term->update_cmd_speed('y', y_speed); term->update_cmd_speed('y', y_speed);
@ -97,10 +102,8 @@ class Run
term->update_nav_data(a, s, time); term->update_nav_data(a, s, time);
} }
void operator()() void operator()() {
{ while (ros::ok()) {
while (ros::ok())
{
ros::spinOnce(); ros::spinOnce();
geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist()); geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist());
@ -109,170 +112,144 @@ class Run
char c = term->getchar(); char c = term->getchar();
switch(c) switch (c) {
{ case 'k' : { // hover
case 'k' :
{// hover
cmd.publish(msg); cmd.publish(msg);
term->log_sent("hover !"); term->log_sent("hover !");
break; break;
} }
case 'i' : case 'i' : { // forward
{// forward
msg->linear.x = x_speed; msg->linear.x = x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("forward !"); term->log_sent("forward !");
break; break;
} }
case ';' : case ';' : { // backward
{// backward
msg->linear.x = -x_speed; msg->linear.x = -x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("backward !"); term->log_sent("backward !");
break; break;
} }
case 'h' : case 'h' : { // translate left
{//translate left
msg->linear.y = -y_speed; msg->linear.y = -y_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("translate left !"); term->log_sent("translate left !");
break; break;
} }
case 'm' : case 'm' : { // translate right
{//translate right
msg->linear.y = y_speed; msg->linear.y = y_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("translate right !"); term->log_sent("translate right !");
break; break;
} }
case 'j' : case 'j' : { // rotate left
{//rotate left
msg->angular.z = turn; msg->angular.z = turn;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("rotate left !"); term->log_sent("rotate left !");
break; break;
} }
case 'l' : case 'l' : { // rotate right
{//rotate right
msg->angular.z = -turn; msg->angular.z = -turn;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("rotate right !"); term->log_sent("rotate right !");
break; break;
} }
case 'u' : case 'u' : { // turn left
{//turn left
msg->angular.z = turn; msg->angular.z = turn;
msg->linear.x = x_speed; msg->linear.x = x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("forward left !"); term->log_sent("forward left !");
break; break;
} }
case 'o' : case 'o' : { // turn right
{//turn right
msg->angular.z = -turn; msg->angular.z = -turn;
msg->linear.x = x_speed; msg->linear.x = x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("forward right !"); term->log_sent("forward right !");
break; break;
} }
case ',' : case ',' : { // turn left backward
{//turn left backward
msg->angular.z = turn; msg->angular.z = turn;
msg->linear.x = -x_speed; msg->linear.x = -x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("backward left !"); term->log_sent("backward left !");
break; break;
} }
case ':' : case ':' : { // turn right backward
{//turn right backward
msg->angular.z = -turn; msg->angular.z = -turn;
msg->linear.x = -x_speed; msg->linear.x = -x_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("backward right !"); term->log_sent("backward right !");
break; break;
} }
case 'y' : case 'y' : { // up
{//up
msg->linear.z = z_speed; msg->linear.z = z_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("up !"); term->log_sent("up !");
break; break;
} }
case 'n' : case 'n' : { // down
{//down
msg->linear.z = -z_speed; msg->linear.z = -z_speed;
cmd.publish(msg); cmd.publish(msg);
term->log_sent("down !"); term->log_sent("down !");
break; break;
} }
case 't' : case 't' : { // takeoff
{//takeoff
takeoff(); takeoff();
term->log_sent("takeoff !"); term->log_sent("takeoff !");
break; break;
} }
case 'b' : case 'b' : { // land
{//land
land(); land();
term->log_sent("land !"); term->log_sent("land !");
break; break;
} }
case 'g' : case 'g' : { // reset
{//reset
reset(); reset();
term->log_sent("reset !"); term->log_sent("reset !");
break; break;
} }
case 'a' : case 'a' : { // + x_speed
{// + x_speed
x_speed *= 1.1; x_speed *= 1.1;
term->update_cmd_speed('x', x_speed); term->update_cmd_speed('x', x_speed);
break; break;
} }
case 'w' : case 'w' : { // - x_speed
{// - x_speed
x_speed *= 0.9; x_speed *= 0.9;
term->update_cmd_speed('x', x_speed); term->update_cmd_speed('x', x_speed);
break; break;
} }
case 'z' : case 'z' : { // + y_speed
{// + y_speed
y_speed *= 1.1; y_speed *= 1.1;
term->update_cmd_speed('y', y_speed); term->update_cmd_speed('y', y_speed);
break; break;
} }
case 'x' : case 'x' : { // - y_speed
{// - y_speed
y_speed *= 0.9; y_speed *= 0.9;
term->update_cmd_speed('y', y_speed); term->update_cmd_speed('y', y_speed);
break; break;
} }
case 'e' : case 'e' : { // + z_speed
{// + z_speed
z_speed *= 1.1; z_speed *= 1.1;
term->update_cmd_speed('z', z_speed); term->update_cmd_speed('z', z_speed);
break; break;
} }
case 'c' : case 'c' : { // - z_speed
{// - z_speed
z_speed *= 0.9; z_speed *= 0.9;
term->update_cmd_speed('z', z_speed); term->update_cmd_speed('z', z_speed);
break; break;
} }
case 'r' : case 'r' : { // + turn speed
{// + turn speed
turn *= 1.1; turn *= 1.1;
term->update_cmd_speed('t', turn); term->update_cmd_speed('t', turn);
break; break;
} }
case 'v' : case 'v' : { // - turn speed
{// - turn speed
turn *= 0.9; turn *= 0.9;
term->update_cmd_speed('t', turn); term->update_cmd_speed('t', turn);
break; break;
} }
default : default : {
{
cmd.publish(msg); cmd.publish(msg);
term->log_sent("hover !"); term->log_sent("hover !");
} }
@ -280,16 +257,13 @@ class Run
loop_rate.sleep(); loop_rate.sleep();
} // while } // while
} // void run() } // void run()
}; // class Run }; // class Run
int main(int argc, char** argv) int main(int argc, char** argv) {
{
setlocale(LC_ALL, ""); setlocale(LC_ALL, "");
ros::init(argc, argv, "keyboard_cmd"); ros::init(argc, argv, "keyboard_cmd");
boost::shared_ptr<Curses> term(new Curses()); boost::shared_ptr<Curses> term(new Curses());
Run fun(term); Run fun(term);
fun(); fun();
return 0; return 0;
} // main }