better commander_atan (min and max in class Function)

This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-30 15:23:12 +02:00
parent 6c88fb973f
commit 0025af11a1
2 changed files with 68 additions and 75 deletions

View file

@ -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("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("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("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("x_p", 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_min", double_t, 0, "min x coord to be published", 0.1, 0., 1.) 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_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_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_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) gen.add("atan", bool_t, 0, "use atan function instead of linear function", False)
exit(gen.generate(PACKAGE, "hand_control", "Commander_atan")) exit(gen.generate(PACKAGE, "hand_control", "Commander_atan"))

View file

@ -20,13 +20,25 @@ class Run
{ {
private: private:
enum FType { f_lin, f_atan, f_undef };
class Function class Function
{ {
private:
virtual void set_grad(const float& grad) = 0;
protected: protected:
float max; float min, max;
FType type;
public: public:
Function() : min(0.1f), max(0.8f) {}
virtual float f(const float& arg) = 0; 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 class Atan : public Function
@ -34,20 +46,19 @@ class Run
private: private:
static const float demi_pi; static const float demi_pi;
float t; float t;
virtual void set_grad(const float& grad)
{
t = grad*demi_pi/max;
}
public: public:
Atan() :t(demi_pi) { type = f_atan; }
virtual float f(const float& arg) virtual float f(const float& arg)
{ {
return max * atan(t * arg) / demi_pi; float out = max * atan(t * arg) / demi_pi;
} if (fabs(out) > min)
Atan(const float& max, const float& grad) return out;
:t(demi_pi) else
{ return 0.;
set(max, grad);
}
virtual void set(const float& maximum, const float& grad)
{
max = maximum;
t = grad*demi_pi/max;
} }
}; };
@ -55,26 +66,23 @@ class Run
{ {
private: private:
float t; float t;
public: virtual void set_grad(const float& grad)
Lin(const float& max, const float& grad)
{ {
set(max, grad);
}
virtual void set(const float& maximum, const float& grad)
{
max = maximum;
t = grad; t = grad;
} }
public:
Lin() { type = f_lin; }
virtual float f(const float& arg) 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<boost::shared_ptr<Function> > f; Data<boost::shared_ptr<Function> > f;
Data<FType> f_type;
Data<float> min;
float neutral_z; float neutral_z;
float max_curv; float max_curv;
uint64_t min_number; uint64_t min_number;
@ -83,12 +91,10 @@ class Run
public: public:
Run(const ros::Publisher& cmd_publisher) : Run(const ros::Publisher& cmd_publisher) :
pub(cmd_publisher) { pub(cmd_publisher) {
f.x = boost::make_shared<Lin>(0.8f, 0.5f); f.x = boost::make_shared<Lin>();
f.y = boost::make_shared<Lin>(0.8f, 0.5f); f.y = boost::make_shared<Lin>();
f.z = boost::make_shared<Lin>(0.8f, 2.0f); f.z = boost::make_shared<Lin>();
f.th = boost::make_shared<Lin>(0.8f, 0.005f); f.th = boost::make_shared<Lin>();
f_type.x = f_type.y = f_type.z = f_type.th = f_lin;
min.x = min.y = min.z = min.th = 0.1;
} }
void callback(const hand_control::Plan::ConstPtr& msg) void callback(const hand_control::Plan::ConstPtr& msg)
@ -96,14 +102,14 @@ class Run
ROS_INFO("plan received"); ROS_INFO("plan received");
Data<float> in, out; Data<float> in, out;
in.x = in.y = in.z = in.th = 0.; 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->curvature < max_curv && msg->number > min_number)
{ {
if(msg->normal.z > 0) if(msg->normal.z > 0)
{ {
in.y = msg->normal.x; in.y = msg->normal.x;
in.x = msg->normal.y; in.x = msg->normal.y;
} else { } else
{
in.y = - msg->normal.x; in.y = - msg->normal.x;
in.x = - msg->normal.y; in.x = - msg->normal.y;
} }
@ -115,22 +121,15 @@ class Run
if (fabs(in.y) > fabs(in.x)) if (fabs(in.y) > fabs(in.x))
{ {
out.y = f.y->f(in.y); mvt->linear.y = f.y->f(in.y);
if (fabs(out.y) > min.y) } else
mvt->linear.y = out.y; {
} else { mvt->linear.x = f.x->f(in.x);
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); mvt->linear.z = f.z->f(in.z);
if (fabs(out.z) > min.z)
mvt->linear.z = out.z;
out.th = f.th->f(in.th); mvt->angular.z = f.th->f(in.th);
if (fabs(out.th) > min.th)
mvt->angular.z = out.th;
pub.publish(mvt); pub.publish(mvt);
ROS_INFO("cmd published"); ROS_INFO("cmd published");
@ -142,30 +141,24 @@ class Run
neutral_z = c.neutral_alt; neutral_z = c.neutral_alt;
min_number = c.min_points_number; 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<Atan>(0.8f, 0.5f); f.x = boost::make_shared<Atan>();
f.y = boost::make_shared<Atan>(0.8f, 0.5f); f.y = boost::make_shared<Atan>();
f.z = boost::make_shared<Atan>(0.8f, 2.f); f.z = boost::make_shared<Atan>();
f.th = boost::make_shared<Atan>(0.8f, 0.005f); f.th = boost::make_shared<Atan>();
f_type.x = f_type.y = f_type.z = f_type.th = f_atan; } else if (!c.atan and f.x->get_type() == f_atan)
{
} else if (!c.atan and f_type.x == f_atan) { f.x = boost::make_shared<Lin>();
f.y = boost::make_shared<Lin>();
f.x = boost::make_shared<Lin>(0.8f, 0.5f); f.z = boost::make_shared<Lin>();
f.y = boost::make_shared<Lin>(0.8f, 0.5f); f.th = boost::make_shared<Lin>();
f.z = boost::make_shared<Lin>(0.8f, 2.0f);
f.th = boost::make_shared<Lin>(0.8f, 0.005f);
f_type.x = f_type.y = f_type.z = f_type.th = f_lin;
} }
f.x->set(c.max_x, c.p_x); f.x->set(c.x_min, c.x_max, c.x_p);
f.y->set(c.max_y, c.p_y); f.y->set(c.y_min, c.y_max, c.y_p);
f.z->set(c.max_z, c.p_z); f.z->set(c.z_min, c.z_max, c.z_p);
f.th->set(c.max_th, c.p_th); f.th->set(c.th_min, c.th_max, c.th_p);
min.x = c.x_min; ROS_INFO("parameters reconfigured");
min.y = c.y_min;
min.z = c.z_min;
min.th = c.th_min;
} }
}; };