From 264dca57e277ef4a3f71ee356e6600f0a66cdfca Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Wed, 27 May 2015 23:28:43 +0200 Subject: [PATCH] =?UTF-8?q?with=20=C2=ABangle=C2=BB=20&=20=C2=ABtheta?= =?UTF-8?q?=C2=BB=20between=20-90=20and=2090?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cfg/Commander.cfg | 8 +-- src/commander.cpp | 24 ++++---- src/estimator.cpp | 154 ++++++++++++++++++++-------------------------- 3 files changed, 81 insertions(+), 105 deletions(-) diff --git a/cfg/Commander.cfg b/cfg/Commander.cfg index 6fbf640..fb0a605 100755 --- a/cfg/Commander.cfg +++ b/cfg/Commander.cfg @@ -6,11 +6,11 @@ gen.add("max_curvature", double_t, 0, "Maximum curvature of the estimated plane" gen.add("x_minimal_deviation", double_t, 0, "Absolute horizontal movement detection treshold", 0.05, 0.) gen.add("y_minimal_deviation", double_t, 0, "Absolute lateral movement detection treshold", 0.05, 0.) gen.add("z_minimal_deviation", double_t, 0, "Absolute vertical movement detection treshold", 0.1, 0.) -gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 1.5, 0.) -gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 3000, 0) +gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 0.8, 0.) +gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 2000, 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("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 5., 0., 45.) +gen.add("angle_vel", double_t, 0, "Angular velocity", 0.002, 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", "Commander")) diff --git a/src/commander.cpp b/src/commander.cpp index c09cec7..5aed7f1 100644 --- a/src/commander.cpp +++ b/src/commander.cpp @@ -20,8 +20,6 @@ class Run { private: float xx, yy, zz, theta; - float DEMI_PI; - // xx < 0 : forward // xx > 0 : backward @@ -61,10 +59,10 @@ class Run { mvt->linear.x = - xx * plan_vel; } - - if (fabs(theta) > th_dev_min) { - mvt->angular.z = theta * angle_vel; - } + + if (fabs(theta) > th_dev_min) { + mvt->angular.z = theta * angle_vel; + } assert(mvt->linear.x == 0. || mvt->linear.y == 0.); pub.publish(mvt); @@ -74,16 +72,15 @@ class Run public: Run(const ros::Publisher& cmd_publisher) : pub(cmd_publisher) - { - DEMI_PI = 2*atan(1.); - } + { + } void callback(const hand_control::Plan::ConstPtr& msg) { ROS_INFO("plan received"); if (msg->curvature < max_curv && msg->number > min_number) { - + if(msg->normal.z > 0) { yy = msg->normal.x; @@ -97,7 +94,8 @@ class Run zz = msg->altitude - neutral_z; - theta = msg->angle - DEMI_PI; + theta = msg->angle; + // theta between -90 and 90 if (first_msg) { @@ -127,7 +125,7 @@ class Run void run() { - ros::spin(); + ros::spin(); } }; @@ -140,7 +138,7 @@ int main(int argc, char** argv) ros::Publisher cmd_pub = node.advertise("/cmd_vel", 1); Run run(cmd_pub); ros::Subscriber plan_sub = node.subscribe("input", 1, &Run::callback, &run); - + dynamic_reconfigure::Server server; dynamic_reconfigure::Server::CallbackType f; f = boost::bind(&Run::reconfigure, &run, _1, _2); diff --git a/src/estimator.cpp b/src/estimator.cpp index 11c5d6b..80d3b19 100644 --- a/src/estimator.cpp +++ b/src/estimator.cpp @@ -14,118 +14,96 @@ typedef Eigen::Matrix3f& Matrix; class Callback { public: - void - operator()(const PointCloud::ConstPtr& msg) + void operator()(const PointCloud::ConstPtr& msg) { - if (msg->width >3){ - analyser.setInputCloud(msg); - Matrix eg = analyser.getEigenVectors(); ROS_INFO("PointCloud received"); - float x, y, z, th, h, c; - x = y = z = th = h = c = 0.; + if (msg->width > 3){ - // indices : tout le PointCloud - std::vector indices; - for (int i = 0; i < msg->points.size(); ++i) - indices.push_back(i); + analyser.setInputCloud(msg); + Matrix eg = analyser.getEigenVectors(); + float x, y, z, th, h, c; + x = y = z = th = h = c = 0.; + // we consider the whole PointCloud + std::vector indices; + for (int i = 0; i < msg->points.size(); ++i) + indices.push_back(i); - // vérifier signature - //estimator.computePointNormal(*msg, indices, x, y, z, c); - //Produit vectoriel des deux première colonnes de Matrix3f - /* x = eg(2,1)*eg(3,2) - - eg(3,1)*eg(2,2); - y = eg(3,1)*eg(1,2) - - eg(1,1)*eg(3,2); - z = eg(1,1)*eg(2,2) - - eg(2,1)*eg(1,2);*/ - Eigen::Vector3f v = eg.col(0).cross(eg.col(1)); - v.normalize(); - /* - x = x/sqrt(x*x+y*y+z*z); - y = x/sqrt(x*x+y*y+z*z); - z = x/sqrt(x*x+y*y+z*z); - */ - x = v(0); y=v(1); z=v(2); - - - h = (analyser.getMean())(2); - - //h = altitude(msg); - th = - 2 * atan (eg(1,0) - /(1 + eg(0,0))); + // v = eg_1 ^ eg_2 is the plan normal + Eigen::Vector3f v = eg.col(0).cross(eg.col(1)); + // norm(v) == 1 + v.normalize(); + x = v(0); y=v(1); z=v(2); - // publication - ROS_INFO("Plan published"); - publisher.publish(to_Plan(x, y, z, h, th, c, msg->header.seq, msg->header.stamp, msg->width)); + // h is the altitude + h = (analyser.getMean())(2); + + // this formula is good only if : + // -pi/2 <= th <= pi/2 + // ie cos(th) == m_x >= 0 + float m_x(eg(0,0)); + float m_y(eg(1,0)); + if(x < 0) + { + m_x *= -1.; + m_y *= -1.; + } + th = 2 * atan(m_y / (1 + m_x)); + // -pi/2 <= th <= pi/2 + + th *= _RAD2DEG; + // -90 <= th <= 90 + + // 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) {} + Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)){}; private: ros::Publisher publisher; pcl::PCA analyser; - //pcl::NormalEstimationOMP estimator; - - inline - const hand_control::Plan::ConstPtr - to_Plan(const float& x, const float& y, - const float& z, const float& h, - const float& th, - 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; - ros_msg->normal.y = y; - ros_msg->normal.z = z; - ros_msg->altitude = h; - ros_msg->angle = th; - ros_msg->curvature = c; - ros_msg->number = number; - // uint64_t msec64 is in ms (10-6) - uint64_t sec64 = msec64 / 1000000; - uint64_t nsec64 = (msec64 % 1000000) * 1000; - ros_msg->header.stamp.sec = (uint32_t) sec64; - ros_msg->header.stamp.nsec = (uint32_t) nsec64; - ros_msg->header.seq = seq; - ros_msg->header.frame_id = "0"; - return ros_msg; - } + const float _RAD2DEG; -/* inline - float - altitude(const PointCloud::ConstPtr& pcl) - { - int s = pcl->points.size(); - float h(0); - for (int i = 0; i < s; ++i) - h += pcl->points[i].z; - return h/( (float) s ); - - - getmean - getVector4fMap - }*/ + inline const hand_control::Plan::ConstPtr + to_Plan(const float& x, const float& y, + const float& z, const float& h, + const float& th, + 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; + ros_msg->normal.y = y; + ros_msg->normal.z = z; + ros_msg->altitude = h; + ros_msg->angle = th; + ros_msg->curvature = c; + ros_msg->number = number; + // uint64_t msec64 is in ms (10-6) + uint64_t sec64 = msec64 / 1000000; + uint64_t nsec64 = (msec64 % 1000000) * 1000; + ros_msg->header.stamp.sec = (uint32_t) sec64; + ros_msg->header.stamp.nsec = (uint32_t) nsec64; + ros_msg->header.seq = seq; + ros_msg->header.frame_id = "0"; + return ros_msg; + } }; -int -main(int argc, char** argv) +int main(int argc, char** argv) { - ros::init(argc, argv, "normal_estimator_pca"); - ros::NodeHandle node("estimator");//`A vérifier ? + ros::init(argc, argv, "estimator"); + ros::NodeHandle node("estimator"); - // initialisation ros::Publisher publisher = node.advertise("output", 1); Callback callback(publisher); ros::Subscriber subscriber = node.subscribe("input", 1, callback); - // démarrage ROS_INFO("node started"); ros::spin(); ROS_INFO("exit");