diff --git a/CMakeLists.txt b/CMakeLists.txt
index 78bfffb..5597922 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -62,3 +62,9 @@ add_dependencies(normal_estimator hand_control_generate_messages_cpp)
add_executable(commande src/commande.cpp)
target_link_libraries(commande ${catkin_LIBRARIES})
+
+add_executable(commande-new src/commande-new.cpp)
+target_link_libraries(commande-new ${catkin_LIBRARIES})
+
+add_executable(commande-new-1d src/commande-new-1d.cpp)
+target_link_libraries(commande-new-1d ${catkin_LIBRARIES})
diff --git a/launch/all-new-1d.launch b/launch/all-new-1d.launch
new file mode 100644
index 0000000..3704926
--- /dev/null
+++ b/launch/all-new-1d.launch
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/all-new.launch b/launch/all-new.launch
new file mode 100644
index 0000000..49c3677
--- /dev/null
+++ b/launch/all-new.launch
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/commande-new-1d.cpp b/src/commande-new-1d.cpp
new file mode 100644
index 0000000..65fedf6
--- /dev/null
+++ b/src/commande-new-1d.cpp
@@ -0,0 +1,211 @@
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+#include
+
+class Run
+{
+ private:
+ float xx, yy, dz;
+
+ // xx > 0 : forward
+ // xx < 0 : backward
+
+ // yy > 0 : right
+ // yy < 0 : left
+
+ // dz > 0 : up
+ // dz < 0 : down
+
+ float plan_vel, z_vel;
+
+ // to calculate dz
+ float z_current, z_previous;
+ ros::Time t_current, t_previous;
+
+ // conditions to publish a message
+ float max_curv;
+ 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;
+ }
+ if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
+ {
+ mvt->linear.y = yy * plan_vel;
+ }
+ else if (fabs(xx) > x_dev_min)
+ {
+ mvt->linear.x = xx * plan_vel;
+ }
+ pub.publish(mvt);
+ ROS_INFO("cmd published");
+ }//end publish
+
+ public:
+ Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity,
+ const float& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation,
+ const float& dz_minimal_difference, const int& min_points_number) :
+ pub(cmd_publisher),
+ plan_vel(plan_velocity),
+ max_curv(max_curvature),
+ z_vel(z_velocity),
+ xx(0),
+ yy(0),
+ dz(0),
+ x_dev_min(x_minimal_deviation),
+ y_dev_min(y_minimal_deviation),
+ dz_min(dz_minimal_difference),
+ first_msg(true),
+ min_number(min_points_number){
+ z_current = z_previous = std::numeric_limits::signaling_NaN();
+ t_previous.nsec = t_previous.sec =
+ t_previous.nsec = t_previous.sec = std::numeric_limits::signaling_NaN();
+ }
+
+ void callback(const hand_control::Plan::ConstPtr& msg)
+ {
+ ROS_INFO("plan received");
+ if (msg->curvature < max_curv && msg->number > min_number)
+ {
+ t_current = msg->header.stamp;
+ z_current = msg->altitude;
+
+ if (!first_msg)
+ {
+ dz = (z_current - z_previous)/((t_current - t_previous).toSec());
+ ROS_INFO("dz = %f", dz);
+ }
+
+ if(msg->normal.z > 0)
+ {
+ yy = msg->normal.x;
+ xx = msg->normal.y;
+ }
+ else
+ {
+ yy = - msg->normal.x;
+ xx = - msg->normal.y;
+ }
+
+ t_previous = t_current;
+ z_previous = z_current;
+ z_current = std::numeric_limits::signaling_NaN();
+ t_current.nsec = t_current.sec = std::numeric_limits::signaling_NaN();
+ if (first_msg)
+ {
+ first_msg = false;
+ ROS_INFO("first msg received");
+ }
+ ROS_INFO("coords updated");
+ } else {
+ xx = yy = dz = 0.;
+ }
+ publish();
+ };
+
+ void run()
+ {
+ ros::spin();
+ }
+
+};
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "commande");
+ ros::NodeHandle node("commande");
+
+ double max_curv(0);
+ if (node.getParam("max_curv", max_curv))
+ {
+ ROS_INFO("max_curv : %f" , max_curv);
+ } else {
+ node.setParam("max_curv", 0.08);
+ 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);
+ }
+
+ 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", x_dev_min))
+ {
+ ROS_INFO("x_dev_min : %f" , x_dev_min);
+ } else {
+ node.setParam("x_dev_min", 0.05);
+ node.getParam("x_dev_min", x_dev_min);
+ ROS_INFO("x_dev_min : %f (default value)", x_dev_min);
+ }
+
+ double y_dev_min(0);
+ if (node.getParam("y_dev_min", y_dev_min))
+ {
+ ROS_INFO("y_dev_min : %f" , y_dev_min);
+ } else {
+ node.setParam("y_dev_min", 0.05);
+ node.getParam("y_dev_min", y_dev_min);
+ ROS_INFO("y_dev_min : %f (default value)", y_dev_min);
+ }
+
+ double dz_dev_min(0);
+ if (node.getParam("dz_dev_min", dz_dev_min))
+ {
+ ROS_INFO("dz_dev_min : %f" , dz_dev_min);
+ } else {
+ node.setParam("dz_dev_min", 0.05);
+ node.getParam("dz_dev_min", dz_dev_min);
+ ROS_INFO("dz_dev_min : %f (default value)", dz_dev_min);
+ }
+
+ ros::Publisher cmd_pub = node.advertise("/cmd_vel", 1);
+ 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("input", 1, &Run::callback, &run);
+ run.run();
+ return 0;
+}
diff --git a/src/commande-new.cpp b/src/commande-new.cpp
new file mode 100644
index 0000000..f84b743
--- /dev/null
+++ b/src/commande-new.cpp
@@ -0,0 +1,211 @@
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+#include
+
+class Run
+{
+ private:
+ float xx, yy, dz;
+
+ // xx > 0 : forward
+ // xx < 0 : backward
+
+ // yy > 0 : right
+ // yy < 0 : left
+
+ // dz > 0 : up
+ // dz < 0 : down
+
+ float plan_vel, z_vel;
+
+ // to calculate dz
+ float z_current, z_previous;
+ ros::Time t_current, t_previous;
+
+ // conditions to publish a message
+ float max_curv;
+ 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;
+ }
+ if (fabs(xx) > x_dev_min)
+ {
+ mvt->linear.x = xx * plan_vel;
+ }
+ if (fabs(yy) > y_dev_min)
+ {
+ mvt->linear.y = yy * plan_vel;
+ }
+ pub.publish(mvt);
+ ROS_INFO("cmd published");
+ }//end publish
+
+ public:
+ Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity,
+ const float& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation,
+ const float& dz_minimal_difference, const int& min_points_number) :
+ pub(cmd_publisher),
+ plan_vel(plan_velocity),
+ max_curv(max_curvature),
+ z_vel(z_velocity),
+ xx(0),
+ yy(0),
+ dz(0),
+ x_dev_min(x_minimal_deviation),
+ y_dev_min(y_minimal_deviation),
+ dz_min(dz_minimal_difference),
+ first_msg(true),
+ min_number(min_points_number){
+ z_current = z_previous = std::numeric_limits::signaling_NaN();
+ t_previous.nsec = t_previous.sec =
+ t_previous.nsec = t_previous.sec = std::numeric_limits::signaling_NaN();
+ }
+
+ void callback(const hand_control::Plan::ConstPtr& msg)
+ {
+ ROS_INFO("plan received");
+ if (msg->curvature < max_curv && msg->number > min_number)
+ {
+ t_current = msg->header.stamp;
+ z_current = msg->altitude;
+
+ if (!first_msg)
+ {
+ dz = (z_current - z_previous)/((t_current - t_previous).toSec());
+ ROS_INFO("dz = %f", dz);
+ }
+
+ if(msg->normal.z > 0)
+ {
+ yy = msg->normal.x;
+ xx = msg->normal.y;
+ }
+ else
+ {
+ yy = - msg->normal.x;
+ xx = - msg->normal.y;
+ }
+
+ t_previous = t_current;
+ z_previous = z_current;
+ z_current = std::numeric_limits::signaling_NaN();
+ t_current.nsec = t_current.sec = std::numeric_limits::signaling_NaN();
+ if (first_msg)
+ {
+ first_msg = false;
+ ROS_INFO("first msg received");
+ }
+ ROS_INFO("coords updated");
+ } else {
+ xx = yy = dz = 0.;
+ }
+ publish();
+ };
+
+ void run()
+ {
+ ros::spin();
+ }
+
+};
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "commande");
+ ros::NodeHandle node("commande");
+
+ double max_curv(0);
+ if (node.getParam("max_curv", max_curv))
+ {
+ ROS_INFO("max_curv : %f" , max_curv);
+ } else {
+ node.setParam("max_curv", 0.08);
+ 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);
+ }
+
+ 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", x_dev_min))
+ {
+ ROS_INFO("x_dev_min : %f" , x_dev_min);
+ } else {
+ node.setParam("x_dev_min", 0.05);
+ node.getParam("x_dev_min", x_dev_min);
+ ROS_INFO("x_dev_min : %f (default value)", x_dev_min);
+ }
+
+ double y_dev_min(0);
+ if (node.getParam("y_dev_min", y_dev_min))
+ {
+ ROS_INFO("y_dev_min : %f" , y_dev_min);
+ } else {
+ node.setParam("y_dev_min", 0.05);
+ node.getParam("y_dev_min", y_dev_min);
+ ROS_INFO("y_dev_min : %f (default value)", y_dev_min);
+ }
+
+ double dz_dev_min(0);
+ if (node.getParam("dz_dev_min", dz_dev_min))
+ {
+ ROS_INFO("dz_dev_min : %f" , dz_dev_min);
+ } else {
+ node.setParam("dz_dev_min", 0.05);
+ node.getParam("dz_dev_min", dz_dev_min);
+ ROS_INFO("dz_dev_min : %f (default value)", dz_dev_min);
+ }
+
+ ros::Publisher cmd_pub = node.advertise("/cmd_vel", 1);
+ 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("input", 1, &Run::callback, &run);
+ run.run();
+ return 0;
+}