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)
target_link_libraries(pcl_displayer ${catkin_LIBRARIES})
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
## as an example, message headers may need to be generated before nodes
# 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 <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <hand_control/Plan.h>
typedef pcl::PointXYZRGB Point;
@ -13,14 +13,16 @@ class Callback {
operator()(const PointCloud::ConstPtr& msg)
{
ROS_INFO("PointCloud received");
Eigen::Vector4f pcl_coord();
Eigen::Vector4f pcl_coord;
float curvature;
std::vector<int> indices();
// TODO : choisir tous les indices
std::vector<int> indices;
// indices : tout le PointCloud
for (int i = 0; i < msg->points.size(); ++i)
indices.push_back(i);
estimator.computePointNormal(*msg, indices,
pcl_coord, curvature);
// publication
ROS_INFO("Plan published")
ROS_INFO("Plan published");
publisher.publish(to_Plan(pcl_coord, curvature));
}
@ -29,12 +31,13 @@ class Callback {
private:
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
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.y = pcl_coord(1); // b
ros_msg->normal.z = pcl_coord(2); // c
@ -53,7 +56,6 @@ main(int argc, char** argv)
// initialisation
ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1);
Callback callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
// démarrage