diff --git a/src/commande.cpp b/src/commande.cpp index 53fa758..ff92355 100644 --- a/src/commande.cpp +++ b/src/commande.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include @@ -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; - double plan_vel, z_vel; + float xx, yy, dz; + 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; - double dz_min, x_dev_min, y_dev_min; + 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; + 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) : - pub(cmd_pub), - plan_vel(p_vel), - max_curv(M_curve), - z_vel(h_vel), - loop_rate(30), + 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_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(); - } + x_dev_min(x_minimal_deviation), + y_dev_min(y_minimal_deviation), + dz_min(dz_minimal_difference), + first_msg(true), + loop_rate(30), + 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 operator()(const hand_control::Plan::ConstPtr& msg) + void callback(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::signaling_NaN(); + z_current = std::numeric_limits::signaling_NaN(); t_current.nsec = t_current.sec = std::numeric_limits::signaling_NaN(); if (first_msg) { @@ -86,38 +112,13 @@ class Run void run() { - ROS_INFO("node running"); - while(ros::ok()) { - ROS_INFO("loop entered"); - - 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); + publish(); 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("/cmd_vel", 1); - Run run(cmd_pub, max_curv, plan_vel, z_vel, 0.0, 0.0, 0.0); - ros::Subscriber plan_sub = node.subscribe("input", 1, run); + 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/normal_estimator.cpp b/src/normal_estimator.cpp index 8dd4bb1..8cc2f63 100644 --- a/src/normal_estimator.cpp +++ b/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, - uint64_t msec64, uint64_t number) + const float& c, const uint32_t& seq, + const uint64_t& msec64, const uint64_t& number) { hand_control::Plan::Ptr ros_msg(new hand_control::Plan()); ros_msg->normal.x = x;