LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

comments commander.cpp

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-06-26 20:13:29 +02:00
parent 16e9dae1a7
commit 8e0ec310d4
1 changed files with 24 additions and 25 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

49
src/commander.cpp
View file

@ -19,38 +19,34 @@
class Run class Run
{ {
private: private:
float xx, yy, zz, theta; float xx, yy, zz, theta; // read coords
// xx < 0 : forward
// xx > 0 : backward
// yy > 0 : right // see README.md to know what are the parameters
// yy < 0 : left float x_vel, y_vel, z_vel, angle_vel, up_factor, neutral_z; // parameters
float max_curv; // not used yet
// zz > 0 : up float z_dev_min, x_dev_min, y_dev_min, th_dev_min; // parameters : thresholds
// zz < 0 : down uint64_t min_number; // parameter
bool no_diag; // parameter
float x_vel, y_vel, z_vel, angle_vel, up_factor, neutral_z;
float max_curv;
float z_dev_min, x_dev_min, y_dev_min, th_dev_min;
uint64_t min_number;
bool no_diag;
ros::Publisher pub; ros::Publisher pub;
void publish() void publish()
// 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
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
// 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)
@ -61,7 +57,7 @@ class Run
{ {
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
{ {
if (fabs(yy) > y_dev_min) if (fabs(yy) > y_dev_min)
mvt->linear.y = yy * y_vel; mvt->linear.y = yy * y_vel;
@ -73,21 +69,20 @@ class Run
mvt->angular.z = theta * angle_vel; mvt->angular.z = theta * angle_vel;
} }
assert(mvt->linear.x == 0. || mvt->linear.y == 0.);
pub.publish(mvt); pub.publish(mvt);
ROS_INFO("cmd published"); ROS_INFO("cmd published");
}//end publish }
public: public:
Run(const ros::Publisher& cmd_publisher) : Run(const ros::Publisher& cmd_publisher) :
pub(cmd_publisher) pub(cmd_publisher) {}
{
}
void callback(const hand_control::Plan::ConstPtr& msg) void callback(const hand_control::Plan::ConstPtr& msg)
// handle received messages
{ {
ROS_INFO("plan received"); ROS_INFO("plan received");
if (msg->curvature < max_curv && msg->number > min_number) 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) if(msg->normal.z > 0)
@ -113,7 +108,9 @@ class Run
publish(); publish();
}; };
void reconfigure(const hand_control::CommanderConfig& c, const uint32_t& level) { void reconfigure(const hand_control::CommanderConfig& c, const uint32_t& level)
// updates the parameters (received with dynamic_reconfigure)
{
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;
@ -130,6 +127,7 @@ class Run
} }
void run() void run()
// runs the callbacks and publications process
{ {
ros::spin(); ros::spin();
} }
@ -140,16 +138,17 @@ 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)
dynamic_reconfigure::Server<hand_control::CommanderConfig> server; dynamic_reconfigure::Server<hand_control::CommanderConfig> server;
dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f; dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f;
f = boost::bind(&Run::reconfigure, &run, _1, _2); f = boost::bind(&Run::reconfigure, &run, _1, _2);
server.setCallback(f); server.setCallback(f);
// starts working
run.run(); run.run();
return 0; return 0;
} }