better commande node

src/commande.cpp

@@ -1,100 +1,142 @@
 #include <ros/ros.h>
 #include <ros/time.h>
 #include <locale.h>
+#include <limits>
 
 #include <pcl_ros/point_cloud.h>
 #include <pcl/point_types.h>
 
 #include <hand_control/Plan.h>
-
 #include <geometry_msgs/Twist.h>
-
 #include <math.h>
 
 class Run
 {
   private:
-	  float z_previous;
-	  ros::Time t_previous;
-	  ros::Time t_current;	  
     ros::Rate loop_rate;
-	  float dz;
-	  float xx;
-	  float yy;
-	  float zz;
-	  float x_speed;
-	  float y_speed;
-	  float z_speed_max;
 
-	  float precision;
+    // xx*plan_vel, yy*plan_vel and dz*dz_vel will be published
+    double xx, yy, dz;
+    double plan_vel, z_vel;
+
+    // to calculate dz
+    double z_current, z_previous;
+    ros::Time t_current, t_previous;
+
+    // conditions to publish a message
+    double max_curv;
+    double dz_min, x_dev_min, y_dev_min;
+
+    bool first_msg;
+
+    ros::Publisher pub;
 
   public:
-	  Run() :
+    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) :
-	      x_speed(0.2),
+      pub(cmd_pub),
-	      y_speed(0.2),
+      plan_vel(p_vel),
-	      z_speed_max(0.5),
+      max_curv(M_curve),
-	      loop_rate(30)
+      z_vel(h_vel),
-	      {
+      loop_rate(30),
-	      //Sensibilité du drône	      
+      xx(0),
-	      precision = 0.1;   
+      yy(0),
+      dz(0),
+      x_dev_min(x_dev_m),
+      y_dev_min(y_dev_m),
+      dz_min(dz_m),
+      first_msg(true) {
+      z_current = z_previous =  std::numeric_limits<double>::signaling_NaN();
+      t_previous.nsec = t_previous.sec =
+      t_previous.nsec = t_previous.sec = std::numeric_limits<uint32_t>::signaling_NaN();
     }
 
 
     void operator()(const hand_control::Plan::ConstPtr& msg)
+    {
+      if (msg->curvature < max_curv)
       {
         t_current = msg->header.stamp;
+        z_current = msg->normal.z;
         
-		dz = (msg->normal.z - z_previous)/(t_current.sec - t_previous.sec);
+        if (!first_msg)
+          dz = (z_current - z_previous)/((t_current - t_previous).toSec());
 
-		xx = msg->normal.x ; 
+        xx = msg->normal.x;
-		yy = msg->normal.y ; 		
+        yy = msg->normal.y;
-		//zz = msg->normal.z ; 
 
-		t_previous = msg->header.stamp;
+        t_previous = t_current;
-		z_previous = msg->normal.z;      	
+        z_previous = z_current;
+        z_current = std::numeric_limits<double>::signaling_NaN();
+        t_current.nsec = t_current.sec = std::numeric_limits<uint32_t>::signaling_NaN();
+        if (first_msg) first_msg = false;
+      }
     };
 
-	void run(){
+    void run()
-	//pour initialiser mvt (twist)
+    {
-	geometry_msgs::Twist::Ptr mvt_init(new geometry_msgs::Twist());
+      geometry_msgs::Twist zero;
-	mvt_init->linear.x = mvt_init->linear.y = mvt_init->linear.z = 
+      zero.linear.x = zero.linear.y = zero.linear.z = 
-	mvt_init->angular.x = mvt_init->angular.y = mvt_init->angular.z = 0.;
+        zero.angular.x = zero.angular.y = zero.angular.z = 0.;
 
-	
+      while(ros::ok())
-	  while(ros::ok()){
+      {
 
         geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
-		  mvt = mvt_init;
+        *mvt = zero;
 
-		  //mouvement selon z
+        if (dz > dz_min)
-		  if (dz < z_speed_max){  //pour éviter un soulèvement trop prompt du drône
+          mvt->linear.z = dz * z_vel;
-		  mvt->linear.z = dz *0.1 ;
+        if (xx > x_dev_min)
-		  }
+          mvt->linear.x = xx * plan_vel;
-	  
+        if (yy > y_dev_min)
-		  	//mouvement selon x ou y	
+          mvt->linear.y = yy * plan_vel;
-			if (fabs(xx) > precision && fabs(yy) > precision ) 
-			  {
-				mvt->linear.x = x_speed ;
-				mvt->linear.y = y_speed ;			
-			  }
 
+        pub.publish(mvt);
         ros::spinOnce();
         loop_rate.sleep();
       }//end while
     }//end run
 
-}; //class Callback
+};
 
 int main(int argc, char** argv)
 {
-
-  
-  
   ros::init(argc, argv, "commande");
+  ros::NodeHandle node("commande");
 
-  Run run;
+  double max_curv(0);
+  if (node.getParam("max_curv", max_curv))
+  {
+    ROS_INFO("max_curv : %f" , max_curv);
+  } else {
+    node.setParam("max_curv", 0.1);
+    node.getParam("max_curv", max_curv);
+    ROS_INFO("max_curv : %f (default value)", max_curv);
+  }
+
+  double plan_vel(0);
+  if (node.getParam("plan_vel", plan_vel))
+  {
+    ROS_INFO("plan_vel : %f" , plan_vel);
+  } else {
+    node.setParam("plan_vel", 0.8);
+    node.getParam("plan_vel", plan_vel);
+    ROS_INFO("plan_vel : %f (default value)", plan_vel);
+  }
+
+  double z_vel(0);
+  if (node.getParam("z_vel", z_vel))
+  {
+    ROS_INFO("z_vel : %f" , z_vel);
+  } else {
+    node.setParam("z_vel", 0.8);
+    node.getParam("z_vel", z_vel);
+    ROS_INFO("z_vel : %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.05, 0.05, 0.05);
+  ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, run);
   run.run();
-
-  
   return 0;
 }