commande works

src/commande.cpp

@@ -2,6 +2,7 @@
 #include <ros/time.h>
 #include <locale.h>
 #include <limits>
+#include <math.h>
 
 #include <pcl_ros/point_cloud.h>
 #include <pcl/point_types.h>
@@ -13,59 +14,84 @@
 class Run
 {
   private:
+
     ros::Rate loop_rate;
 
     // xx*plan_vel, yy*plan_vel and dz*dz_vel will be published
-    double xx, yy, dz;
+    float xx, yy, dz;
-    double plan_vel, z_vel;
+    float plan_vel, z_vel;
 
     // to calculate dz
-    double z_current, z_previous;
+    float z_current, z_previous;
     ros::Time t_current, t_previous;
 
     // conditions to publish a message
-    double max_curv;
+    float max_curv;
-    double dz_min, x_dev_min, y_dev_min;
+    float dz_min, x_dev_min, y_dev_min;
+    uint64_t min_number;
 
     bool first_msg;
 
     ros::Publisher pub;
 
+    void publish()
+    {
+      geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
+      if (fabs(dz) > dz_min)
+      {
+        mvt->linear.z = dz * z_vel;
+        ROS_INFO("z changed");
+      }
+      if (xx > x_dev_min)
+      {
+        mvt->linear.y = xx * plan_vel;
+        // because of the kinect orientation
+      }
+      if (yy > y_dev_min)
+      {
+        mvt->linear.x = yy * plan_vel;
+        // because of the kinect orientation
+      }
+      pub.publish(mvt);
+      ROS_INFO("cmd published");
+    }//end publish
+
   public:
-    Run(ros::Publisher cmd_pub, double M_curve, double p_vel, double h_vel, double x_dev_m, double y_dev_m, double dz_m) :
+    Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity,
-      pub(cmd_pub),
+        const float& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation,
-      plan_vel(p_vel),
+        const float& dz_minimal_difference, const int& min_points_number) :
-      max_curv(M_curve),
+      pub(cmd_publisher),
-      z_vel(h_vel),
+      plan_vel(plan_velocity),
-      loop_rate(30),
+      max_curv(max_curvature),
+      z_vel(z_velocity),
       xx(0),
       yy(0),
       dz(0),
-      x_dev_min(x_dev_m),
+      x_dev_min(x_minimal_deviation),
-      y_dev_min(y_dev_m),
+      y_dev_min(y_minimal_deviation),
-      dz_min(dz_m),
+      dz_min(dz_minimal_difference),
-      first_msg(true) {
+      first_msg(true),
-      z_current = z_previous =  std::numeric_limits<double>::signaling_NaN();
+      loop_rate(30),
-      t_previous.nsec = t_previous.sec =
+      min_number(min_points_number){
-      t_previous.nsec = t_previous.sec = std::numeric_limits<uint32_t>::signaling_NaN();
+        z_current = z_previous =  std::numeric_limits<float>::signaling_NaN();
-    }
+        t_previous.nsec = t_previous.sec =
+          t_previous.nsec = t_previous.sec = std::numeric_limits<uint32_t>::signaling_NaN();
+      }
 
-
+    void callback(const hand_control::Plan::ConstPtr& msg)
-    void operator()(const hand_control::Plan::ConstPtr& msg)
     {
       ROS_INFO("plan received");
-      if (msg->curvature < max_curv)
+      if (msg->curvature < max_curv && msg->number > min_number)
       {
         t_current = msg->header.stamp;
         z_current = msg->normal.z;
-        
+
         if (!first_msg)
         {
           dz = (z_current - z_previous)/((t_current - t_previous).toSec());
           ROS_INFO("dz = %f", dz);
         }
 
-
         xx = msg->normal.x;
         ROS_INFO("xx = %f", xx);
         yy = msg->normal.y;
@@ -73,7 +99,7 @@ class Run
 
         t_previous = t_current;
         z_previous = z_current;
-        z_current = std::numeric_limits<double>::signaling_NaN();
+        z_current = std::numeric_limits<float>::signaling_NaN();
         t_current.nsec = t_current.sec = std::numeric_limits<uint32_t>::signaling_NaN();
         if (first_msg)
         {
@@ -86,38 +112,13 @@ class Run
 
     void run()
     {
-      ROS_INFO("node running");
-
       while(ros::ok())
       {
-        ROS_INFO("loop entered");
+        publish();
-
-        geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
-        
-        ROS_INFO("dz : %f > %f : dz_min ?", dz, dz_min);
-        if (dz > dz_min)
-        {
-          mvt->linear.z = dz * z_vel;
-          ROS_INFO("z changed");
-        }
-        ROS_INFO("xx : %f > %f : x_dev_min ?", xx, x_dev_min);
-        if (xx > x_dev_min)
-        {
-          mvt->linear.x = xx * plan_vel;
-          ROS_INFO("x changed");
-        }
-        ROS_INFO("yy : %f > %f : y_dev_min ?", yy, y_dev_min);
-        if (yy > y_dev_min)
-        {
-          mvt->linear.y = yy * plan_vel;
-          ROS_INFO("y changed");
-        }
-
-        pub.publish(mvt);
         ros::spinOnce();
         loop_rate.sleep();
-      }//end while
+      }
-    }//end run
+    }
 
 };
 
@@ -131,7 +132,7 @@ int main(int argc, char** argv)
   {
     ROS_INFO("max_curv : %f" , max_curv);
   } else {
-    node.setParam("max_curv", 0.1);
+    node.setParam("max_curv", 0.08);
     node.getParam("max_curv", max_curv);
     ROS_INFO("max_curv : %f (default value)", max_curv);
   }
@@ -156,9 +157,49 @@ int main(int argc, char** argv)
     ROS_INFO("z_vel : %f (default value)", z_vel);
   }
 
+  int min_number(0);
+  if (node.getParam("min_number", min_number))
+  {
+    ROS_INFO("min_number : %d" , min_number);
+  } else {
+    node.setParam("min_number", 500);
+    node.getParam("min_number", min_number);
+    ROS_INFO("min_number : %d (default value)", min_number);
+  }
+
+  double x_dev_min(0);
+  if (node.getParam("x_dev_min", z_vel))
+  {
+    ROS_INFO("x_dev_min : %f" , z_vel);
+  } else {
+    node.setParam("x_dev_min", 0.05);
+    node.getParam("x_dev_min", z_vel);
+    ROS_INFO("x_dev_min : %f (default value)", z_vel);
+  }
+
+  double y_dev_min(0);
+  if (node.getParam("y_dev_min", z_vel))
+  {
+    ROS_INFO("y_dev_min : %f" , z_vel);
+  } else {
+    node.setParam("y_dev_min", 0.05);
+    node.getParam("y_dev_min", z_vel);
+    ROS_INFO("y_dev_min : %f (default value)", z_vel);
+  }
+
+  double dz_dev_min(0);
+  if (node.getParam("dz_dev_min", z_vel))
+  {
+    ROS_INFO("dz_dev_min : %f" , z_vel);
+  } else {
+    node.setParam("dz_dev_min", 0.05);
+    node.getParam("dz_dev_min", z_vel);
+    ROS_INFO("dz_dev_min : %f (default value)", z_vel);
+  }
+
   ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
-  Run run(cmd_pub, max_curv, plan_vel, z_vel, 0.0, 0.0, 0.0);
+  Run run(cmd_pub, max_curv, plan_vel, z_vel, x_dev_min, y_dev_min, dz_dev_min, min_number);
-  ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, run);
+  ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
   run.run();
   return 0;
 }

src/normal_estimator.cpp

@@ -43,8 +43,8 @@ class Callback {
     const hand_control::Plan::ConstPtr
     to_Plan(const float& x, const float& y,
             const float& z, const float& h,
-            const float& c, uint32_t seq,
+            const float& c, const uint32_t& seq,
-            uint64_t msec64, uint64_t number)
+            const uint64_t& msec64, const uint64_t& number)
     {
       hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
       ros_msg->normal.x = x;