adds linear fonction to commander_atan

cfg/Commander_atan.cfg

@@ -7,8 +7,12 @@ gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement co
 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", 0.5, 0.)
-gen.add("p_th", double_t, 0, "th gradient in 0", 0.01, 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_min", double_t, 0, "min x coord to be published", 0.1, 0., 1.)
 gen.add("y_min", double_t, 0, "min y coord to be published", 0.1, 0., 1.)
 gen.add("z_min", double_t, 0, "min z coord to be published", 0.1, 0., 1.)

src/commander_atan.cpp

@@ -19,29 +19,63 @@
 class Run
 {
   private:
+
     class Function
+    {
+      protected:
+        float max;
+        float gradient;
+      public:
+        virtual float f(const float& arg) = 0;
+        Function() : max(0.8) {}
+        Function(const float& maximum, const float& grad)
+        {
+          set_gradient(grad);
+          set_max(maximum);
+        }
+        virtual void set_gradient(const float& grad)
+        {
+          gradient = grad;
+        }
+        void set_max(const float& m)
+        {
+          max = m;
+        }
+    };
+
+    class Atan : public Function
     {
       private:
-        float t;
         static const float demi_pi;
+        float t;
       public:
-        float operator()(float arg)
+        virtual float f(const float& arg)
         {
-          return atan(t * arg) / demi_pi;
+          return max * atan(t * arg) / demi_pi;
         }
-        Function() : t(demi_pi){}
-        void set_t(float tt)
+        Atan(const float& max, const float& grad)
+          :t(demi_pi)
         {
-          t = tt;
+          set_gradient(grad);
         }
-        void set_p(float pp)
-          // pp : gradient in 0
+        virtual void set_gradient(const float& grad)
         {
-          t = pp*demi_pi;
+          t = grad*2*atan(1);
+          gradient = grad;
         }
     };
 
-    Data<Function> f;
+    class Lin : public Function
+    {
+      public:
+        Lin(const float& max, const float& grad){}
+        virtual float f(const float& arg)
+        {
+          return std::max(gradient * arg, max);
+        }
+    };
+
+    Data<boost::shared_ptr<Function> > f;
     Data<float> min;
     float neutral_z;
     float max_curv;
@@ -51,7 +85,10 @@ class Run
   public:
     Run(const ros::Publisher& cmd_publisher) :
       pub(cmd_publisher) {
-        f.x = f.y = f.z = f.th = Function();
+        f.x = boost::make_shared<Lin>(0.8f, 0.5f);
+        f.y = boost::make_shared<Lin>(0.8f, 0.5f);
+        f.z = boost::make_shared<Lin>(0.8f, 2.0f);
+        f.th = boost::make_shared<Lin>(0.8f, 0.005f);
         min.x = min.y = min.z = min.th = 0.1;
       }
 
@@ -77,22 +114,22 @@ class Run
 
       geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
 
-      //      if (fabs(in.y) > fabs(in.x))
-      //     {
-      out.y = f.y(in.y);
-      if (fabs(out.y) > min.y)
-        mvt->linear.y = out.y;
-      //    } else {
-      out.x = f.x(in.x);
-      if (fabs(out.x) > min.x)
-        mvt->linear.x = out.x;
-      //   }
-
-      out.z = f.z(in.z);
+      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;
+      }
+
+      out.z = f.z->f(in.z);
       if (fabs(out.z) > min.z)
         mvt->linear.z = out.z;
 
-      out.th = f.th(in.th);
+      out.th = f.th->f(in.th);
       if (fabs(out.th) > min.th)
         mvt->angular.z = out.th;
 
@@ -105,10 +142,14 @@ class Run
       max_curv = c.max_curvature;
       neutral_z = c.neutral_alt;
       min_number = c.min_points_number;
-      f.x.set_p(c.p_x);
-      f.y.set_p(c.p_y);
-      f.z.set_p(c.p_z);
-      f.th.set_p(c.p_th);
+      f.x->set_gradient(c.p_x);
+      f.y->set_gradient(c.p_y);
+      f.z->set_gradient(c.p_z);
+      f.th->set_gradient(c.p_th);
+      f.x->set_max(c.max_x);
+      f.y->set_max(c.max_y);
+      f.z->set_max(c.max_z);
+      f.th->set_max(c.max_th);
       min.x = c.x_min;
       min.y = c.y_min;
       min.z = c.z_min;
@@ -136,4 +177,4 @@ int main(int argc, char** argv)
   return 0;
 }
 
-const float Run::Function::demi_pi = 2*atan(1.);
+const float Run::Atan::demi_pi = 2*atan(1);