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,67 +16,62 @@
* 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 <assert.h>
#include <locale.h>
#include <ros/ros.h>
#include <ros/time.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <hand_control/Plan.h>
#include <geometry_msgs/Twist.h>
#include <math.h>
#include <dynamic_reconfigure/server.h>
#include <hand_control/CommanderConfig.h>
#include <limits>
class Run
{
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 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
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
{
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)
{
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 ;
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 (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
{
} 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)
@ -92,43 +87,35 @@ class Run
}
public:
Run(const ros::Publisher& cmd_publisher) :
explicit Run(const ros::Publisher& cmd_publisher) :
pub(cmd_publisher) {}
void callback(const hand_control::Plan::ConstPtr& msg)
// handle received messages
{
void callback(const hand_control::Plan::ConstPtr& msg) {
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;
xx = msg->normal.y;
}
else
{
} 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)
{
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;
@ -144,21 +131,20 @@ class Run
no_diag = c.no_diag;
}
void run()
// runs the callbacks and publications process
{
void run() {
ros::spin();
}
};
int main(int argc, char** argv)
{
int main(int argc, char** argv) {
ros::init(argc, argv, "commander");
ros::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);
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)
dynamic_reconfigure::Server<hand_control::CommanderConfig> server;

View file

@ -17,10 +17,12 @@
*/
// library used by "keyboard_cmd.cpp"
#include "./display.h"
#include <ncurses.h>
#include <string>
#include <geometry_msgs/Twist.h>
#include "display.h"
#include <string>
const int Curses::cmd_kbd_lines = 8;
const int Curses::cmd_kbd_columns = 55;
@ -44,7 +46,7 @@ Curses::Curses() {
initscr();
cbreak();
start_color();
//noecho();
// noecho();
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) {
switch(coord) {
switch (coord) {
case 'x':
wmove(cmd_speed, 0, 16);
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);
break;
default:
;
{}
}
wrefresh(cmd_speed);
}
@ -170,7 +172,7 @@ 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;
@ -202,7 +204,7 @@ void Curses::update_nav_data(const float& batteryPercent,
waddstr(nav_data, "looping ");
break;
default:
;
{}
}
wrefresh(nav_data);
}

View file

@ -16,15 +16,14 @@
* along with Hand Control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef CURSES_DISPLAY
#define CURSES_DISPLAY
#ifndef SRC_DISPLAY_H_
#define SRC_DISPLAY_H_
#include <ncurses.h>
#include <string>
#include <geometry_msgs/Twist.h>
#include <string>
class Curses
{
class Curses {
private:
static const int cmd_kbd_lines;
static const int cmd_kbd_columns;
@ -61,7 +60,6 @@ class 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_

View file

@ -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 <http://www.gnu.org/licenses/>.
*/
@ -34,14 +38,12 @@ typedef Eigen::Matrix3f& Matrix;
class Callback {
public:
void callback(const PointCloud::ConstPtr& msg)
// handles received messages and publish the plan estimation
{
void callback(const PointCloud::ConstPtr& msg) {
ROS_INFO("PointCloud received");
if (msg->width > 3){
if (msg->width > 3) {
// else, no plan can be estimated…
analyser.setInputCloud(msg);
Matrix eg = analyser.getEigenVectors();
@ -54,8 +56,7 @@ class Callback {
v.normalize(); // to have norm(v) == 1
// x, y and z are the coords of the plan normal
if (!reverse)
{
if (!reverse) {
x = v(0); y = v(1);
} else {
// if "x" and "y" axes are inverted
@ -67,46 +68,58 @@ class Callback {
// 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);
if (reverse_angle) {
m_x = eg(0, 0);
m_y = eg(1, 0);
} else {
m_x = eg(1,0);
m_y = eg(0,0);
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 = - asin(m_y / sqrt(pow(m_y, 2)+ pow(m_x, 2))); // 0 <= th <= pi
th *= _RAD2DEG; // -90 <= th <= 90
// TODO
// 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));
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 ;
void reconfigure(const hand_control::EstimatorConfig& c,
const uint32_t& level) {
reverse = c.reverse;
reverse_angle = c.reverse_angle;
}
@ -123,15 +136,16 @@ class Callback {
const float& z, const float& h,
const float& th,
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());
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;
@ -143,13 +157,13 @@ class Callback {
}
};
int main(int argc, char** argv)
{
int main(int argc, char** argv) {
ros::init(argc, argv, "estimator");
ros::NodeHandle node("estimator");
ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1);
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
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/>.
*/
#include <assert.h>
#include <hand_control/FilterConfig.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/tracking/impl/hsv_color_coherence.hpp>
#include <assert.h>
#include <dynamic_reconfigure/server.h>
#include <hand_control/FilterConfig.h>
#include <algorithm>
typedef pcl::PointXYZRGB Point;
@ -30,33 +32,41 @@ typedef pcl::PointCloud<Point> PointCloud;
class Callback {
public:
void
callback(const PointCloud::ConstPtr& msg)
// handles and filters the received PointCloud and
// publishes the filtered PointCloud
{
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)
{
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)
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
{
void reconfigure(const hand_control::FilterConfig& c,
const uint32_t& level) {
z_max = c.z_max;
hue = c.hue;
delta_hue = c.delta_hue;
@ -68,29 +78,27 @@ class Callback {
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)
{
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
{
// 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 ;
h *= 360.0f;
diff1 = std::fabs(h - hue);
// hue is periodic
if (h < hue)
@ -101,14 +109,19 @@ class Callback {
}
};
int
main(int argc, char** argv)
{
int main(int argc, char** argv) {
ros::init(argc, argv, "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);
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
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/>.
*/
#include <ros/ros.h>
#include <ros/time.h>
#include <locale.h>
#include "display.h"
#include <std_msgs/Empty.h>
#include <geometry_msgs/Twist.h>
#include <ardrone_autonomy/Navdata.h>
class NavdataCallback
{
#include "./display.h"
class NavdataCallback {
private:
boost::shared_ptr<Curses> term;
public:
NavdataCallback(const boost::shared_ptr<Curses>& terminal) :
explicit NavdataCallback(const boost::shared_ptr<Curses>& terminal) :
term(terminal) {}
void operator()(const ardrone_autonomy::Navdata::ConstPtr& msg) {
term->update_nav_data(msg->batteryPercent, msg->state, msg->tm);
}
}; // class NavdataCallback
};
class CmdVelCallback
{
class CmdVelCallback {
private:
boost::shared_ptr<Curses> term;
public:
CmdVelCallback(const boost::shared_ptr<Curses>& terminal) :
explicit CmdVelCallback(const boost::shared_ptr<Curses>& terminal) :
term(terminal) {}
void operator()(const geometry_msgs::Twist::ConstPtr& msg) {
term->update_topic(msg);
}
}; // class CmdVelCallback
};
class Run
{
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<Curses> term;
NavdataCallback data_callback;
CmdVelCallback cmdvel_callback;
public:
Run(const boost::shared_ptr<Curses>& terminal) :
explicit Run(const boost::shared_ptr<Curses>& terminal) :
data_callback(terminal),
cmdvel_callback(terminal),
term(terminal),
@ -79,12 +78,18 @@ class Run
y_speed(0.1),
z_speed(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_land = n.advertise<std_msgs::Empty>("/ardrone/land", 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('y', y_speed);
@ -97,10 +102,8 @@ class Run
term->update_nav_data(a, s, time);
}
void operator()()
{
while (ros::ok())
{
void operator()() {
while (ros::ok()) {
ros::spinOnce();
geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist());
@ -109,187 +112,158 @@ class Run
char c = term->getchar();
switch(c)
{
case 'k' :
{// hover
switch (c) {
case 'k' : { // hover
cmd.publish(msg);
term->log_sent("hover !");
break;
}
case 'i' :
{// forward
case 'i' : { // forward
msg->linear.x = x_speed;
cmd.publish(msg);
term->log_sent("forward !");
break;
}
case ';' :
{// backward
case ';' : { // backward
msg->linear.x = -x_speed;
cmd.publish(msg);
term->log_sent("backward !");
break;
}
case 'h' :
{//translate left
case 'h' : { // translate left
msg->linear.y = -y_speed;
cmd.publish(msg);
term->log_sent("translate left !");
break;
}
case 'm' :
{//translate right
case 'm' : { // translate right
msg->linear.y = y_speed;
cmd.publish(msg);
term->log_sent("translate right !");
break;
}
case 'j' :
{//rotate left
case 'j' : { // rotate left
msg->angular.z = turn;
cmd.publish(msg);
term->log_sent("rotate left !");
break;
}
case 'l' :
{//rotate right
case 'l' : { // rotate right
msg->angular.z = -turn;
cmd.publish(msg);
term->log_sent("rotate right !");
break;
}
case 'u' :
{//turn left
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
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
case ',' : { // turn left backward
msg->angular.z = turn;
msg->linear.x = -x_speed;
cmd.publish(msg);
term->log_sent("backward left !");
term->log_sent("backward left !");
break;
}
case ':' :
{//turn right backward
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
case 'y' : { // up
msg->linear.z = z_speed;
cmd.publish(msg);
term->log_sent("up !");
break;
}
case 'n' :
{//down
case 'n' : { // down
msg->linear.z = -z_speed;
cmd.publish(msg);
term->log_sent("down !");
break;
}
case 't' :
{//takeoff
case 't' : { // takeoff
takeoff();
term->log_sent("takeoff !");
break;
}
case 'b' :
{//land
case 'b' : { // land
land();
term->log_sent("land !");
break;
}
case 'g' :
{//reset
case 'g' : { // reset
reset();
term->log_sent("reset !");
break;
}
case 'a' :
{// + x_speed
case 'a' : { // + x_speed
x_speed *= 1.1;
term->update_cmd_speed('x', x_speed);
break;
}
case 'w' :
{// - x_speed
case 'w' : { // - x_speed
x_speed *= 0.9;
term->update_cmd_speed('x', x_speed);
break;
}
case 'z' :
{// + y_speed
case 'z' : { // + y_speed
y_speed *= 1.1;
term->update_cmd_speed('y', y_speed);
break;
}
case 'x' :
{// - y_speed
case 'x' : { // - y_speed
y_speed *= 0.9;
term->update_cmd_speed('y', y_speed);
break;
}
case 'e' :
{// + z_speed
case 'e' : { // + z_speed
z_speed *= 1.1;
term->update_cmd_speed('z', z_speed);
break;
}
case 'c' :
{// - z_speed
case 'c' : { // - z_speed
z_speed *= 0.9;
term->update_cmd_speed('z', z_speed);
break;
}
case 'r' :
{// + turn speed
case 'r' : { // + turn speed
turn *= 1.1;
term->update_cmd_speed('t', turn);
break;
}
case 'v' :
{// - turn speed
case 'v' : { // - turn speed
turn *= 0.9;
term->update_cmd_speed('t', turn);
break;
}
default :
{
default : {
cmd.publish(msg);
term->log_sent("hover !");
}
} // switch(c)
loop_rate.sleep();
} // while
} //void run()
} // void run()
}; // class Run
int main(int argc, char** argv)
{
int main(int argc, char** argv) {
setlocale(LC_ALL, "");
ros::init(argc, argv, "keyboard_cmd");
boost::shared_ptr<Curses> term(new Curses());
Run fun(term);
fun();
return 0;
} // main
}