LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

It compiles

Browse source
This commit is contained in:
_Luc_ 2015-05-27 18:31:41 +02:00
parent f8113fc265
commit d7897cf7d0
2 changed files with 23 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
CMakeLists.txt
View file

@ -53,7 +53,7 @@ target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRAR
target_link_libraries(normal_estimator ${catkin_LIBRARIES}) target_link_libraries(normal_estimator ${catkin_LIBRARIES})
add_dependencies(normal_estimator hand_control_generate_messages_cpp) add_dependencies(normal_estimator hand_control_generate_messages_cpp)
add_executable(normal_estimator_pca src/normal_estimator_pca.cpp) add_executable(normal_estimator_pca src/normal_estimator-pca.cpp)
target_link_libraries(normal_estimator_pca ${catkin_LIBRARIES}) target_link_libraries(normal_estimator_pca ${catkin_LIBRARIES})
add_executable(commande-new-1d src/commande-new-1d.cpp) add_executable(commande-new-1d src/commande-new-1d.cpp)

33
src/normal_estimator-pca.cpp
View file

@ -1,7 +1,7 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <pcl_ros/point_cloud.h> #include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
//#include <pcl/features/normal_3d_omp.h> #include <pcl/features/normal_3d_omp.h>
#include <hand_control/Plan.h> #include <hand_control/Plan.h>
#include <time.h> #include <time.h>
#include <math.h> #include <math.h>
@ -10,12 +10,15 @@
typedef pcl::PointXYZRGB Point; typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud; typedef pcl::PointCloud<Point> PointCloud;
typedef Eigen::Matrix3f& Matrix;
class Callback { class Callback {
public: public:
void void
operator()(const PointCloud::ConstPtr& msg) operator()(const PointCloud::ConstPtr& msg)
{ {
analyser.setInputCloud(msg);
Matrix eg = analyser.getEigenVectors();
ROS_INFO("PointCloud received"); ROS_INFO("PointCloud received");
float x, y, z, th, h, c; float x, y, z, th, h, c;
@ -26,25 +29,33 @@ class Callback {
for (int i = 0; i < msg->points.size(); ++i) for (int i = 0; i < msg->points.size(); ++i)
indices.push_back(i); indices.push_back(i);
// vérifier signature // vérifier signature
//estimator.computePointNormal(*msg, indices, x, y, z, c); //estimator.computePointNormal(*msg, indices, x, y, z, c);
//Produit vectoriel des deux première colonnes de Matrix3f //Produit vectoriel des deux première colonnes de Matrix3f
x = Matrix3f.getElement(2,1)*Matrix3f.getElement(3,2) /* x = eg(2,1)*eg(3,2)
- Matrix3f.getElement(3,1)*Matrix3f.getElement(2,2); - eg(3,1)*eg(2,2);
y = Matrix3f.getElement(3,1)*Matrix3f.getElement(1,2) y = eg(3,1)*eg(1,2)
- Matrix3f.getElement(1,1)*Matrix3f.getElement(3,2); - eg(1,1)*eg(3,2);
z = Matrix3f.getElement(1,1)*Matrix3f.getElement(2,2) z = eg(1,1)*eg(2,2)
- Matrix3f.getElement(2,1)*Matrix3f.getElement(1,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); x = x/sqrt(x*x+y*y+z*z);
y = 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); z = x/sqrt(x*x+y*y+z*z);
*/
x = v(0); y=v(1); z=v(2);
h = pcl::PCA::getMean(msg).z;
h = (analyser.getMean())(3);
//h = altitude(msg); //h = altitude(msg);
th = -90 th = -90
+ 2 * atan (Matrix3f.getElement(2,1) + 2 * atan (eg(2,1)
/(1 + Matrix3f.getElement(1,1))); /(1 + eg(1,1)));
// publication // publication
ROS_INFO("Plan published"); ROS_INFO("Plan published");
@ -56,8 +67,8 @@ class Callback {
private: private:
ros::Publisher publisher; ros::Publisher publisher;
pcl::PCA<Point> analyser;
//pcl::NormalEstimationOMP<Point, pcl::Normal> estimator; //pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
Eigen::Matrix3f& pcl::PCA<Point, pcl::Normal>::getEigenVectors eigenVectors;
inline inline
const hand_control::Plan::ConstPtr const hand_control::Plan::ConstPtr