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

normal_estimator compile

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-04-23 20:42:40 +02:00
parent cf08433335
commit 7cc091e6b1
2 changed files with 14 additions and 8 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
hand_control/CMakeLists.txt
View file

@ -121,6 +121,10 @@ add_dependencies(random_pcl_publisher ${catkin_EXPORTED_TARGETS})
add_executable(pcl_displayer src/pcl_displayer.cpp) add_executable(pcl_displayer src/pcl_displayer.cpp)
target_link_libraries(pcl_displayer ${catkin_LIBRARIES}) target_link_libraries(pcl_displayer ${catkin_LIBRARIES})
add_dependencies(pcl_displayer ${catkin_EXPORTED_TARGETS}) add_dependencies(pcl_displayer ${catkin_EXPORTED_TARGETS})
add_executable(normal_estimator src/normal_estimator.cpp)
target_link_libraries(normal_estimator ${catkin_LIBRARIES})
add_dependencies(normal_estimator ${catkin_EXPORTED_TARGETS})
## Add cmake target dependencies of the executable/library ## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes ## as an example, message headers may need to be generated before nodes
# add_dependencies(hand_control_node hand_control_generate_messages_cpp) # add_dependencies(hand_control_node hand_control_generate_messages_cpp)

18
hand_control/src/normal_estimator.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.h> #include <pcl/features/normal_3d_omp.h>
#include <hand_control/Plan.h> #include <hand_control/Plan.h>
typedef pcl::PointXYZRGB Point; typedef pcl::PointXYZRGB Point;
@ -13,14 +13,16 @@ class Callback {
operator()(const PointCloud::ConstPtr& msg) operator()(const PointCloud::ConstPtr& msg)
{ {
ROS_INFO("PointCloud received"); ROS_INFO("PointCloud received");
Eigen::Vector4f pcl_coord(); Eigen::Vector4f pcl_coord;
float curvature; float curvature;
std::vector<int> indices(); std::vector<int> indices;
// TODO : choisir tous les indices // indices : tout le PointCloud
for (int i = 0; i < msg->points.size(); ++i)
indices.push_back(i);
estimator.computePointNormal(*msg, indices, estimator.computePointNormal(*msg, indices,
pcl_coord, curvature); pcl_coord, curvature);
// publication // publication
ROS_INFO("Plan published") ROS_INFO("Plan published");
publisher.publish(to_Plan(pcl_coord, curvature)); publisher.publish(to_Plan(pcl_coord, curvature));
} }
@ -29,12 +31,13 @@ class Callback {
private: private:
ros::Publisher publisher; ros::Publisher publisher;
pcl::NormalEstimationOMP<Point, Eigen::Vector4f> estimator; //pcl::NormalEstimationOMP<Point, Eigen::Vector4f> estimator;
pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
const hand_control::Plan::ConstPtr const hand_control::Plan::ConstPtr
to_Plan(const Eigen::Vector4f& pcl_coord, const float& curvature) to_Plan(const Eigen::Vector4f& pcl_coord, const float& curvature)
{ {
hand_control::Plan::Ptr ros_msg(new Plan()); hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
ros_msg->normal.x = pcl_coord(0); // a ros_msg->normal.x = pcl_coord(0); // a
ros_msg->normal.y = pcl_coord(1); // b ros_msg->normal.y = pcl_coord(1); // b
ros_msg->normal.z = pcl_coord(2); // c ros_msg->normal.z = pcl_coord(2); // c
@ -53,7 +56,6 @@ main(int argc, char** argv)
// initialisation // initialisation
ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1); ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1);
Callback callback(publisher); Callback callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback); ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
// démarrage // démarrage