adds commander_atan node to replace commander node

CMakeLists.txt

@@ -53,6 +53,10 @@ target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRAR
 target_link_libraries(estimator ${catkin_LIBRARIES})
 add_dependencies(estimator hand_control_generate_messages_cpp)
 
+  add_executable(commander_atan src/commander_atan.cpp)
+target_link_libraries(commander_atan ${catkin_LIBRARIES})
+add_dependencies(commander_atan hand_control_generate_messages_cpp)
+
   add_executable(commander src/commander.cpp)
 target_link_libraries(commander ${catkin_LIBRARIES})
 add_dependencies(commander hand_control_generate_messages_cpp)
@@ -62,11 +66,13 @@ generate_dynamic_reconfigure_options(
   cfg/Filter.cfg
   cfg/Commander.cfg
   cfg/Estimator.cfg
+  cfg/Commander_atan.cfg
   )
 
 add_dependencies(filter ${PROJECT_NAME}_gencfg)
 add_dependencies(commander ${PROJECT_NAME}_gencfg)
 add_dependencies(estimator ${PROJECT_NAME}_gencfg)
+add_dependencies(commander_atan ${PROJECT_NAME}_gencfg)
 
   catkin_package(
                  CATKIN_DEPENDS message_runtime

cfg/Commander.cfg

@@ -3,14 +3,14 @@ PACKAGE = "hand_control"
 from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
 gen.add("max_curvature", double_t, 0, "Maximum curvature of the estimated plane", 0.5, 0., 1.)
-gen.add("x_minimal_deviation", double_t, 0, "Absolute horizontal movement detection treshold", 0.05, 0.)
-gen.add("y_minimal_deviation", double_t, 0, "Absolute lateral movement detection treshold", 0.05, 0.)
+gen.add("x_minimal_deviation", double_t, 0, "Absolute horizontal movement detection treshold", 0.1, 0.)
+gen.add("y_minimal_deviation", double_t, 0, "Absolute lateral movement detection treshold", 0.1, 0.)
 gen.add("z_minimal_deviation", double_t, 0, "Absolute vertical movement detection treshold", 0.1, 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", 2000, 0)
+gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 1000, 0)
 gen.add("up_fact", double_t, 0, "Upward command amplification factor", 1.5, 1)
-gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 5., 0., 45.)
-gen.add("angle_vel", double_t, 0, "Angular velocity", 0.002, 0., 10.)
-gen.add("plan_vel", double_t, 0, "Translation velocity", 2, 0., 10.)
-gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2, 0., 10.)
+gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 15., 0., 45.)
+gen.add("angle_vel", double_t, 0, "Angular velocity", 0.01, 0., 10.)
+gen.add("plan_vel", double_t, 0, "Translation velocity", 0.5, 0., 10.)
+gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2., 0., 10.)
 exit(gen.generate(PACKAGE, "hand_control", "Commander"))

cfg/Commander_atan.cfg

@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+PACKAGE = "hand_control"
+from dynamic_reconfigure.parameter_generator_catkin import *
+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)
+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("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.)
+gen.add("th_min", double_t, 0, "min th coord to be published", 0.1, 0., 1.)
+exit(gen.generate(PACKAGE, "hand_control", "Commander_atan"))

src/commander_atan.cpp

@@ -0,0 +1,140 @@
+#include <ros/ros.h>
+#include <ros/time.h>
+#include <locale.h>
+#include <limits>
+#include <math.h>
+#include <assert.h>
+
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <hand_control/Plan.h>
+#include <geometry_msgs/Twist.h>
+#include <math.h>
+
+#include <dynamic_reconfigure/server.h>
+#include <hand_control/Commander_atanConfig.h>
+#include "data.h"
+
+class Run
+{
+  private:
+    class Function
+    {
+      private:
+        float t;
+        static const float demi_pi;
+      public:
+        float operator()(float arg)
+        {
+          return atan(t * arg) / demi_pi;
+        }
+        Function() : t(demi_pi){}
+        void set_t(float tt)
+        {
+          t = tt;
+        }
+        void set_p(float pp)
+          // pp : gradient in 0
+        {
+          t = pp*demi_pi;
+        }
+    };
+
+    Data<Function> f;
+    Data<float> min;
+    float neutral_z;
+    float max_curv;
+    uint64_t min_number;
+    ros::Publisher pub;
+
+  public:
+    Run(const ros::Publisher& cmd_publisher) :
+      pub(cmd_publisher) {
+        f.x = f.y = f.z = f.th = Function();
+        min.x = min.y = min.z = min.th = 0.1;
+      }
+
+    void callback(const hand_control::Plan::ConstPtr& msg)
+    {
+      ROS_INFO("plan received");
+      Data<float> 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 {
+          in.y = - msg->normal.x;
+          in.x = - msg->normal.y;
+        }
+        in.z = msg->altitude - neutral_z;
+        in.th = msg->angle;
+      }
+
+      geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
+
+      if (fabs(in.y) > fabs(in.x))
+      {
+        out.y = f.y(in.y);
+        if (out.y > min.y)
+          mvt->linear.y = out.y;
+      } else {      {
+        out.x = f.x(in.x);
+        if (out.x > min.x)
+          mvt->linear.x = out.x;
+      }
+
+      out.z = f.z(in.z);
+      if (out.z > min.z)
+        mvt->linear.z = out.z;
+
+      out.th = f.th(in.th);
+      if (out.th > min.th)
+        mvt->angular.z = out.th;
+
+      pub.publish(mvt);
+      ROS_INFO("cmd published");
+      }
+    }
+
+    void reconfigure(const hand_control::Commander_atanConfig& c, const uint32_t& level)
+    {
+      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);
+      min.x = c.x_min;
+      min.y = c.y_min;
+      min.z = c.z_min;
+      min.th = c.th_min;
+    }  
+
+};
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "commander_atan");
+  ros::NodeHandle node("commander_atan");
+
+  ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
+  Run run(cmd_pub);
+  ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
+
+  dynamic_reconfigure::Server<hand_control::Commander_atanConfig> server;
+  dynamic_reconfigure::Server<hand_control::Commander_atanConfig>::CallbackType f;
+  f = boost::bind(&Run::reconfigure, &run, _1, _2);
+  server.setCallback(f);
+
+  ROS_INFO("start spinning");
+  ros::spin();
+  return 0;
+}
+
+const float Run::Function::demi_pi = 2*atan(1.);

src/data.h

@@ -0,0 +1,9 @@
+#ifndef DATA_H
+#define DATA_H
+template <typename T>
+struct Data
+{
+  public:
+    T x, y, z, th;
+};
+#endif