Solving + Param

cfg/Commande.cfg

@@ -10,4 +10,7 @@ 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", 3000, 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", 15., 0., 180.)
+gen.add("angle_vel", double_t, 0, "Angular velocity", 0.5, 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.)
 exit(gen.generate(PACKAGE, "hand_control", "Commande"))

launch/all-abs.launch

@@ -0,0 +1,19 @@
+
+<launch>
+
+  <include file="$(find hand_control)/launch/all-param.launch" />
+
+  <include file="$(find hand_control)/launch/freenect-registered-xyzrgb.launch"/>
+  <node name="filtre" pkg="hand_control" type="filtre">
+    <remap from="/filtre/input" to="/camera/xyzrgb/points"/>
+  </node>
+
+  <node name="estimator" pkg="hand_control" type="normal_estimator_pca">
+    <remap from="/estimator/input" to="/filtre/output"/>
+  </node>
+
+  <node name="commander" pkg="hand_control" type="commande-abs">
+    <remap from="/commande/input" to="/estimator/output"/>
+  </node>
+
+</launch>

src/commande-abs.cpp

@@ -65,7 +65,7 @@ class Run
        mvt->angular.z * angle_vel;
      }
 
-      assert(mvt->linear.x == 0. || mvt->linear.y == 0.);
+      assert(mvt->linear.x != 0. || mvt->linear.y != 0.);
       pub.publish(mvt);
       ROS_INFO("cmd published");
     }//end publish
@@ -117,6 +117,9 @@ class Run
       neutral_z = c.neutral_alt;
       min_number = c.min_points_number;
       up_factor = c.up_fact;
+      plan_vel = c.plan_vel;
+      z_vel = c.z_vel;
+      angle_vel = c.angle_vel;
     }  
 
     void run()

src/normal_estimator-pca.cpp

@@ -17,6 +17,7 @@ class Callback {
     void
     operator()(const PointCloud::ConstPtr& msg)
     {
+      if (msg->width >3){
       analyser.setInputCloud(msg);
       Matrix eg = analyser.getEigenVectors();
       ROS_INFO("PointCloud received");
@@ -50,17 +51,18 @@ class Callback {
            x = v(0); y=v(1); z=v(2);
        
       
-      h = (analyser.getMean())(3);
+      h = (analyser.getMean())(2);
       
       //h = altitude(msg);
-      th = -90
+      th = 
-           + 2 * atan (eg(2,1)
+            2 * atan (eg(2,1)
                        /(1 + eg(1,1)));
 
       // publication
       ROS_INFO("Plan published");
       publisher.publish(to_Plan(x, y, z, h, th, c, msg->header.seq, msg->header.stamp, msg->width));
       }
+    }
 
     Callback(ros::Publisher& pub) :
       publisher(pub) {}