complies with cpplint
This commit is contained in:
parent
f94492264d
commit
15b11c3bb5
6 changed files with 635 additions and 648 deletions
|
@ -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;
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
|
@ -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_
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in a new issue