LGD
/
hand_control
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

complies with cpplint

master
Louis-Guillaume DUBOIS 10 years ago
parent
f94492264d
commit
15b11c3bb5
6 changed files with 648 additions and 661 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 82
      src/commander.cpp
  2. 16
      src/display.cpp
  3. 12
      src/display.h
  4. 62
      src/estimator.cpp
  5. 77
      src/filter.cpp
  6. 136
      src/keyboard_cmd.cpp

82
src/commander.cpp
View File

@ -16,67 +16,62 @@
* 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;
else else
mvt->linear.z = zz * z_vel; mvt->linear.z = zz * z_vel;
} }
// no_diag true : the drone can only translate on the "x" axis // 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;

16
src/display.cpp
View File

@ -17,10 +17,12 @@
*/ */
// library used by "keyboard_cmd.cpp" // library used by "keyboard_cmd.cpp"
#include "./display.h"
#include <ncurses.h> #include <ncurses.h>
#include <string>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include "display.h" #include <string>
const int Curses::cmd_kbd_lines = 8; const int Curses::cmd_kbd_lines = 8;
const int Curses::cmd_kbd_columns = 55; const int Curses::cmd_kbd_columns = 55;
@ -44,7 +46,7 @@ Curses::Curses() {
initscr(); initscr();
cbreak(); cbreak();
start_color(); start_color();
//noecho(); // noecho();
cmd_kbd = newwin(cmd_kbd_lines, cmd_kbd_columns, 0, 0); cmd_kbd = newwin(cmd_kbd_lines, cmd_kbd_columns, 0, 0);
@ -126,7 +128,7 @@ void Curses::print_cmd_speed() {
} }
void Curses::update_cmd_speed(const char& coord, const float& v) { void Curses::update_cmd_speed(const char& coord, const float& v) {
switch(coord) { switch (coord) {
case 'x': case 'x':
wmove(cmd_speed, 0, 16); wmove(cmd_speed, 0, 16);
wprintw(cmd_speed, "%f", v); wprintw(cmd_speed, "%f", v);
@ -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);
} }
@ -170,7 +172,7 @@ void Curses::update_nav_data(const float& batteryPercent,
wmove(nav_data, 2, 10); wmove(nav_data, 2, 10);
wprintw(nav_data, "%f %", time); wprintw(nav_data, "%f %", time);
wmove(nav_data, 1, 10); wmove(nav_data, 1, 10);
switch(state) { switch (state) {
case 0: case 0:
waddstr(nav_data, "unknown "); waddstr(nav_data, "unknown ");
break; break;
@ -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);
} }

12
src/display.h
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_

62
src/estimator.cpp
View File

@ -1,17 +1,21 @@
/* Copyright © 2015 CentraleSupélec /* Copyright © 2015 CentraleSupélec
* *
* This file is part of Hand Control. * This file is part of Hand Control.
* *
* Hand Control is free software: you can redistribute it and/or modify * Hand Control is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or * the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version. * (at your option) any later version.
* *
* Hand Control is distributed in the hope that it will be useful, * Hand Control is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU General Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU General Public License
* along with Hand Control. If not, see <http://www.gnu.org/licenses/>. * along with Hand Control. If not, see <http://www.gnu.org/licenses/>.
*/ */
@ -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,46 +68,58 @@ 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 {
m_x = eg(1,0); m_x = eg(1, 0);
m_y = eg(0,0); m_y = eg(0, 0);
} }
// because we want "th" only between -90° and 90° // because we want "th" only between -90° and 90°
if (m_x < 0.) if (m_x < 0.)
m_y *= -1; m_y *= -1;
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,
reverse = c.reverse ; const uint32_t& level) {
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;

77
src/filter.cpp
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,29 +78,27 @@ 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;
diff1 = std::fabs(h - hue); diff1 = std::fabs(h - hue);
// hue is periodic // hue is periodic
if (h < hue) if (h < hue)
@ -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;

136
src/keyboard_cmd.cpp
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),
@ -79,12 +78,18 @@ class Run
y_speed(0.1), y_speed(0.1),
z_speed(0.1), z_speed(0.1),
turn(0.1) { turn(0.1) {
cmd = n.advertise<geometry_msgs::Twist>("/cmd_vel",1); cmd = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
pub_takeoff = n.advertise<std_msgs::Empty>("/ardrone/takeoff", 1); pub_takeoff = n.advertise<std_msgs::Empty>("/ardrone/takeoff", 1);
pub_land = n.advertise<std_msgs::Empty>("/ardrone/land", 1); pub_land = n.advertise<std_msgs::Empty>("/ardrone/land", 1);
pub_reset = n.advertise<std_msgs::Empty>("/ardrone/reset", 1); pub_reset = n.advertise<std_msgs::Empty>("/ardrone/reset", 1);
data_subscriber = n.subscribe<ardrone_autonomy::Navdata>("/ardrone/navdata", 1, data_callback);
cmdvel_subscriber = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, cmdvel_callback); 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,187 +112,158 @@ 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 !");
} }
} // switch(c) } // switch(c)
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 }

Write Preview
Loading…
Cancel
Save