From 0025af11a1fa34e1cdc4588c68b0a90e6618f036 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 30 May 2015 15:23:12 +0200 Subject: [PATCH] better commander_atan (min and max in class Function) --- cfg/Commander_atan.cfg | 16 +++--- src/commander_atan.cpp | 127 +++++++++++++++++++---------------------- 2 files changed, 68 insertions(+), 75 deletions(-) diff --git a/cfg/Commander_atan.cfg b/cfg/Commander_atan.cfg index 6cbc567..b9516e8 100755 --- a/cfg/Commander_atan.cfg +++ b/cfg/Commander_atan.cfg @@ -5,17 +5,17 @@ gen = ParameterGenerator() gen.add("max_curvature", double_t, 0, "Maximum curvature of the estimated plane", 0.5, 0., 1.) gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 0.8, 0.) gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 1000, 0, 50000) -gen.add("p_x", double_t, 0, "x gradient in 0", 0.5, 0.) -gen.add("p_y", double_t, 0, "y gradient in 0", 0.5, 0.) -gen.add("p_z", double_t, 0, "z gradient in 0", 1, 0.) -gen.add("p_th", double_t, 0, "th gradient in 0", 0.005, 0.) -gen.add("max_x", double_t, 0, "max x coord to be published", 0.8, 0., 1.) -gen.add("max_y", double_t, 0, "max y coord to be published", 0.8, 0., 1.) -gen.add("max_z", double_t, 0, "max z coord to be published", 0.8, 0., 1.) -gen.add("max_th", double_t, 0, "max th coord to be published", 0.8, 0., 1.) +gen.add("x_p", double_t, 0, "x gradient in 0", 0.5, 0.) gen.add("x_min", double_t, 0, "min x coord to be published", 0.1, 0., 1.) +gen.add("x_max", double_t, 0, "max x coord to be published", 0.8, 0., 1.) +gen.add("y_p", double_t, 0, "y gradient in 0", 0.5, 0.) gen.add("y_min", double_t, 0, "min y coord to be published", 0.1, 0., 1.) +gen.add("y_max", double_t, 0, "max y coord to be published", 0.8, 0., 1.) +gen.add("z_p", double_t, 0, "z gradient in 0", 1, 0.) gen.add("z_min", double_t, 0, "min z coord to be published", 0.1, 0., 1.) +gen.add("z_max", double_t, 0, "max z coord to be published", 0.8, 0., 1.) +gen.add("th_p", double_t, 0, "th gradient in 0", 0.005, 0.) gen.add("th_min", double_t, 0, "min th coord to be published", 0.1, 0., 1.) +gen.add("th_max", double_t, 0, "max th coord to be published", 0.8, 0., 1.) gen.add("atan", bool_t, 0, "use atan function instead of linear function", False) exit(gen.generate(PACKAGE, "hand_control", "Commander_atan")) diff --git a/src/commander_atan.cpp b/src/commander_atan.cpp index f002efc..3032f3b 100644 --- a/src/commander_atan.cpp +++ b/src/commander_atan.cpp @@ -20,13 +20,25 @@ class Run { private: + enum FType { f_lin, f_atan, f_undef }; + class Function { + private: + virtual void set_grad(const float& grad) = 0; protected: - float max; + float min, max; + FType type; public: + Function() : min(0.1f), max(0.8f) {} virtual float f(const float& arg) = 0; - virtual void set(const float& maximum, const float& grad) = 0; + void set(const float& minimum, const float& maximum, const float& grad) + { + min = minimum; + max = maximum; + set_grad(grad); + } + FType get_type() { return type; } }; class Atan : public Function @@ -34,20 +46,19 @@ class Run private: static const float demi_pi; float t; + virtual void set_grad(const float& grad) + { + t = grad*demi_pi/max; + } public: + Atan() :t(demi_pi) { type = f_atan; } virtual float f(const float& arg) { - return max * atan(t * arg) / demi_pi; - } - Atan(const float& max, const float& grad) - :t(demi_pi) - { - set(max, grad); - } - virtual void set(const float& maximum, const float& grad) - { - max = maximum; - t = grad*demi_pi/max; + float out = max * atan(t * arg) / demi_pi; + if (fabs(out) > min) + return out; + else + return 0.; } }; @@ -55,26 +66,23 @@ class Run { private: float t; - public: - Lin(const float& max, const float& grad) + virtual void set_grad(const float& grad) { - set(max, grad); - } - virtual void set(const float& maximum, const float& grad) - { - max = maximum; t = grad; } + public: + Lin() { type = f_lin; } virtual float f(const float& arg) { - return std::max(t * arg, max); + float out = std::min(t * arg, max); + if (fabs(out) > min) + return out; + else + return 0.; } }; - enum FType { f_lin, f_atan }; Data > f; - Data f_type; - Data min; float neutral_z; float max_curv; uint64_t min_number; @@ -83,12 +91,10 @@ class Run public: Run(const ros::Publisher& cmd_publisher) : pub(cmd_publisher) { - f.x = boost::make_shared(0.8f, 0.5f); - f.y = boost::make_shared(0.8f, 0.5f); - f.z = boost::make_shared(0.8f, 2.0f); - f.th = boost::make_shared(0.8f, 0.005f); - f_type.x = f_type.y = f_type.z = f_type.th = f_lin; - min.x = min.y = min.z = min.th = 0.1; + f.x = boost::make_shared(); + f.y = boost::make_shared(); + f.z = boost::make_shared(); + f.th = boost::make_shared(); } void callback(const hand_control::Plan::ConstPtr& msg) @@ -96,14 +102,14 @@ class Run ROS_INFO("plan received"); Data in, out; in.x = in.y = in.z = in.th = 0.; - out.x = out.y = out.z = out.th = 0; if (msg->curvature < max_curv && msg->number > min_number) { if(msg->normal.z > 0) { in.y = msg->normal.x; in.x = msg->normal.y; - } else { + } else + { in.y = - msg->normal.x; in.x = - msg->normal.y; } @@ -115,22 +121,15 @@ class Run if (fabs(in.y) > fabs(in.x)) { - out.y = f.y->f(in.y); - if (fabs(out.y) > min.y) - mvt->linear.y = out.y; - } else { - out.x = f.x->f(in.x); - if (fabs(out.x) > min.x) - mvt->linear.x = out.x; + mvt->linear.y = f.y->f(in.y); + } else + { + mvt->linear.x = f.x->f(in.x); } - out.z = f.z->f(in.z); - if (fabs(out.z) > min.z) - mvt->linear.z = out.z; + mvt->linear.z = f.z->f(in.z); - out.th = f.th->f(in.th); - if (fabs(out.th) > min.th) - mvt->angular.z = out.th; + mvt->angular.z = f.th->f(in.th); pub.publish(mvt); ROS_INFO("cmd published"); @@ -142,30 +141,24 @@ class Run neutral_z = c.neutral_alt; min_number = c.min_points_number; - if (c.atan and f_type.x != f_atan) + if (c.atan and f.x->get_type() != f_atan) { - f.x = boost::make_shared(0.8f, 0.5f); - f.y = boost::make_shared(0.8f, 0.5f); - f.z = boost::make_shared(0.8f, 2.f); - f.th = boost::make_shared(0.8f, 0.005f); - f_type.x = f_type.y = f_type.z = f_type.th = f_atan; - - } else if (!c.atan and f_type.x == f_atan) { - - f.x = boost::make_shared(0.8f, 0.5f); - f.y = boost::make_shared(0.8f, 0.5f); - f.z = boost::make_shared(0.8f, 2.0f); - f.th = boost::make_shared(0.8f, 0.005f); - f_type.x = f_type.y = f_type.z = f_type.th = f_lin; + f.x = boost::make_shared(); + f.y = boost::make_shared(); + f.z = boost::make_shared(); + f.th = boost::make_shared(); + } else if (!c.atan and f.x->get_type() == f_atan) + { + f.x = boost::make_shared(); + f.y = boost::make_shared(); + f.z = boost::make_shared(); + f.th = boost::make_shared(); } - f.x->set(c.max_x, c.p_x); - f.y->set(c.max_y, c.p_y); - f.z->set(c.max_z, c.p_z); - f.th->set(c.max_th, c.p_th); - min.x = c.x_min; - min.y = c.y_min; - min.z = c.z_min; - min.th = c.th_min; + f.x->set(c.x_min, c.x_max, c.x_p); + f.y->set(c.y_min, c.y_max, c.y_p); + f.z->set(c.z_min, c.z_max, c.z_p); + f.th->set(c.th_min, c.th_max, c.th_p); + ROS_INFO("parameters reconfigured"); } };