diff --git a/src/commander.cpp b/src/commander.cpp index 73f86f5..26a180c 100644 --- a/src/commander.cpp +++ b/src/commander.cpp @@ -19,38 +19,34 @@ class Run { private: - float xx, yy, zz, theta; - // xx < 0 : forward - // xx > 0 : backward + float xx, yy, zz, theta; // read coords - // yy > 0 : right - // yy < 0 : left - - // zz > 0 : up - // zz < 0 : down - - 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; + // 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() + // 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) { + // 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; } - + + + // 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 (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min) @@ -61,7 +57,7 @@ class Run { 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) mvt->linear.y = yy * y_vel; @@ -73,21 +69,20 @@ class Run mvt->angular.z = theta * angle_vel; } - assert(mvt->linear.x == 0. || mvt->linear.y == 0.); pub.publish(mvt); ROS_INFO("cmd published"); - }//end publish + } public: Run(const ros::Publisher& cmd_publisher) : - pub(cmd_publisher) - { - } + pub(cmd_publisher) {} void callback(const hand_control::Plan::ConstPtr& msg) + // handle received messages { 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) @@ -113,7 +108,9 @@ class Run 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; x_dev_min = c.x_minimal_deviation; y_dev_min = c.y_minimal_deviation; @@ -130,6 +127,7 @@ class Run } void run() + // runs the callbacks and publications process { ros::spin(); } @@ -140,16 +138,17 @@ int main(int argc, char** argv) { ros::init(argc, argv, "commander"); ros::NodeHandle node("commander"); - ros::Publisher cmd_pub = node.advertise("/cmd_vel", 1); Run run(cmd_pub); ros::Subscriber plan_sub = node.subscribe("input", 1, &Run::callback, &run); + // setting up dynamic_reconfigure (for rqt_reconfigure) dynamic_reconfigure::Server server; dynamic_reconfigure::Server::CallbackType f; f = boost::bind(&Run::reconfigure, &run, _1, _2); server.setCallback(f); + // starts working run.run(); return 0; }