hand_control/src/commander.cpp

173 lines
4.7 KiB
C++
Raw Normal View History

2015-09-18 14:52:52 +00:00
/* 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/>.
2015-08-21 18:39:01 +00:00
*/
2015-09-18 14:52:52 +00:00
#include <ros/ros.h>
#include <ros/time.h>
#include <locale.h>
#include <limits>
#include <math.h>
#include <assert.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>
class Run
{
private:
2015-06-26 18:13:29 +00:00
float xx, yy, zz, theta; // read coords
2015-06-26 18:13:29 +00:00
// see README.md to know what are the parameters
float x_vel, y_vel, z_vel, angle_vel, up_factor, neutral_z; // parameters
float max_curv; // not used yet
float z_dev_min, x_dev_min, y_dev_min, th_dev_min; // parameters : thresholds
uint64_t min_number; // parameter
bool no_diag; // parameter
ros::Publisher pub;
void publish()
2015-06-26 18:13:29 +00:00
// 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)
{
2015-06-26 18:13:29 +00:00
// up_factor to balance out the gravity effect
if (zz > 0)
mvt->linear.z = zz * z_vel * up_factor ;
else
mvt->linear.z = zz * z_vel;
}
2015-06-26 18:13:29 +00:00
// 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.
2015-06-09 15:00:45 +00:00
if (no_diag)
{
2015-06-09 15:00:45 +00:00
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;
}
2015-06-26 18:13:29 +00:00
} else // no_diag false : the drone can translate on any possible direction
{
2015-06-09 15:00:45 +00:00
if (fabs(yy) > y_dev_min)
mvt->linear.y = yy * y_vel;
if (fabs(xx) > x_dev_min)
mvt->linear.x = - xx * x_vel;
}
if (fabs(theta) > th_dev_min) {
mvt->angular.z = theta * angle_vel;
}
pub.publish(mvt);
ROS_INFO("cmd published");
2015-06-26 18:13:29 +00:00
}
public:
2015-05-27 17:48:06 +00:00
Run(const ros::Publisher& cmd_publisher) :
2015-06-26 18:13:29 +00:00
pub(cmd_publisher) {}
void callback(const hand_control::Plan::ConstPtr& msg)
2015-06-26 18:13:29 +00:00
// handle received messages
{
ROS_INFO("plan received");
if (msg->curvature < max_curv && msg->number > min_number)
2015-06-26 18:13:29 +00:00
// we ever have msg->curvature == 0 in fact (not implemented yet)
{
if(msg->normal.z > 0)
{
yy = msg->normal.x;
xx = msg->normal.y;
}
else
{
yy = - msg->normal.x;
xx = - msg->normal.y;
}
zz = msg->altitude - neutral_z;
theta = msg->angle;
// theta between -90 and 90
ROS_INFO("coords updated");
} else {
xx = yy = zz = 0.;
}
publish();
};
2015-06-26 18:13:29 +00:00
void reconfigure(const hand_control::CommanderConfig& c, const uint32_t& level)
// updates the parameters (received with dynamic_reconfigure)
{
max_curv = c.max_curvature;
x_dev_min = c.x_minimal_deviation;
y_dev_min = c.y_minimal_deviation;
z_dev_min = c.z_minimal_deviation;
th_dev_min = c.theta_minimal_deviation;
neutral_z = c.neutral_alt;
min_number = c.min_points_number;
up_factor = c.up_fact;
x_vel = c.x_vel;
y_vel = c.y_vel;
2015-05-27 18:10:33 +00:00
z_vel = c.z_vel;
angle_vel = c.angle_vel;
2015-06-09 15:00:45 +00:00
no_diag = c.no_diag;
}
void run()
2015-06-26 18:13:29 +00:00
// runs the callbacks and publications process
{
ros::spin();
}
};
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);
2015-05-27 17:48:06 +00:00
Run run(cmd_pub);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
2015-06-26 18:13:29 +00:00
// setting up dynamic_reconfigure (for rqt_reconfigure)
dynamic_reconfigure::Server<hand_control::CommanderConfig> server;
dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f;
f = boost::bind(&Run::reconfigure, &run, _1, _2);
server.setCallback(f);
2015-06-26 18:13:29 +00:00
// starts working
run.run();
return 0;
}