From 97c07c8eaa900a51452335fb3cc3854c99636f67 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Thu, 7 May 2015 19:36:36 +0200 Subject: [PATCH] better commande node --- src/commande.cpp | 198 ++++++++++++++++++++++++++++------------------- 1 file changed, 120 insertions(+), 78 deletions(-) diff --git a/src/commande.cpp b/src/commande.cpp index beb35db..82722a7 100644 --- a/src/commande.cpp +++ b/src/commande.cpp @@ -1,100 +1,142 @@ #include #include #include +#include #include #include #include - #include - #include 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; - - public: - Run() : - x_speed(0.2), - y_speed(0.2), - z_speed_max(0.5), - loop_rate(30) - { - //Sensibilité du drône - precision = 0.1; - } - + private: + ros::Rate loop_rate; - void operator()(const hand_control::Plan::ConstPtr& msg) - { - t_current = msg->header.stamp; - - dz = (msg->normal.z - z_previous)/(t_current.sec - t_previous.sec); - - xx = msg->normal.x ; - yy = msg->normal.y ; - //zz = msg->normal.z ; - - t_previous = msg->header.stamp; - z_previous = msg->normal.z; - }; + // xx*plan_vel, yy*plan_vel and dz*dz_vel will be published + double xx, yy, dz; + double plan_vel, z_vel; - void run(){ - //pour initialiser mvt (twist) - geometry_msgs::Twist::Ptr mvt_init(new geometry_msgs::Twist()); - mvt_init->linear.x = mvt_init->linear.y = mvt_init->linear.z = - mvt_init->angular.x = mvt_init->angular.y = mvt_init->angular.z = 0.; - - - while(ros::ok()){ - - geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist()); - mvt = mvt_init; - - //mouvement selon z - if (dz < z_speed_max){ //pour éviter un soulèvement trop prompt du drône - mvt->linear.z = dz *0.1 ; - } - - //mouvement selon x ou y - if (fabs(xx) > precision && fabs(yy) > precision ) - { - mvt->linear.x = x_speed ; - mvt->linear.y = y_speed ; - } - - ros::spinOnce(); - loop_rate.sleep(); - }//end while - }//end run - -}; //class Callback + // 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(ros::Publisher cmd_pub, double M_curve, double p_vel, double h_vel, double x_dev_m, double y_dev_m, double dz_m) : + pub(cmd_pub), + plan_vel(p_vel), + max_curv(M_curve), + z_vel(h_vel), + loop_rate(30), + xx(0), + 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::signaling_NaN(); + t_previous.nsec = t_previous.sec = + t_previous.nsec = t_previous.sec = std::numeric_limits::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; + + if (!first_msg) + dz = (z_current - z_previous)/((t_current - t_previous).toSec()); + + xx = msg->normal.x; + yy = 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; + } + }; + + void run() + { + geometry_msgs::Twist zero; + zero.linear.x = zero.linear.y = zero.linear.z = + zero.angular.x = zero.angular.y = zero.angular.z = 0.; + + while(ros::ok()) + { + + geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist()); + *mvt = zero; + + if (dz > dz_min) + mvt->linear.z = dz * z_vel; + if (xx > x_dev_min) + mvt->linear.x = xx * plan_vel; + if (yy > y_dev_min) + mvt->linear.y = yy * plan_vel; + + pub.publish(mvt); + ros::spinOnce(); + loop_rate.sleep(); + }//end while + }//end run + +}; int main(int argc, char** argv) { - - - ros::init(argc, argv, "commande"); - - Run run; - run.run(); + 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.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("/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("input", 1, run); + run.run(); return 0; }