diff --git a/hand_control/CMakeLists.txt b/hand_control/CMakeLists.txt
index 87e7100..49d245f 100644
--- a/hand_control/CMakeLists.txt
+++ b/hand_control/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
freenect_camera
freenect_launch
+ message_generation
)
## System dependencies are found with CMake's conventions
@@ -49,11 +50,10 @@ find_package(catkin REQUIRED COMPONENTS
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
-# add_message_files(
-# FILES
-# Message1.msg
-# Message2.msg
-# )
+ add_message_files(
+ FILES
+ Plan.msg
+ )
## Generate services in the 'srv' folder
# add_service_files(
@@ -70,10 +70,11 @@ find_package(catkin REQUIRED COMPONENTS
# )
## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# std_msgs
-# )
+ generate_messages(
+ DEPENDENCIES
+ std_msgs
+ geometry_msgs
+ )
###################################
## catkin specific configuration ##
@@ -87,7 +88,7 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES hand_control
-# CATKIN_DEPENDS ardrone_autonomy freenect_stack pcl_ros perception_pcl roscpp std_msgs
+ CATKIN_DEPENDS ardrone_autonomy freenect_stack pcl_ros perception_pcl roscpp std_msgs message_runtime
# DEPENDS system_lib
)
diff --git a/hand_control/msg/Plan.msg b/hand_control/msg/Plan.msg
new file mode 100644
index 0000000..9b30a24
--- /dev/null
+++ b/hand_control/msg/Plan.msg
@@ -0,0 +1,3 @@
+geometry_msgs/Point normal
+float64 altitude
+float64 curvature
diff --git a/hand_control/package.xml b/hand_control/package.xml
index 46a48cd..dd5511e 100644
--- a/hand_control/package.xml
+++ b/hand_control/package.xml
@@ -26,6 +26,11 @@
pcl_conversions
roscpp
std_msgs
+ message_generation
+ freenect_stack
+ perception_pcl
+ perception_pcl
+ freenect_stack
ardrone_autonomy
freenect_camera
freenect_launch
@@ -35,6 +40,7 @@
pcl_conversions
roscpp
std_msgs
+ message_runtime
diff --git a/hand_control/src/normal_estimator.cpp b/hand_control/src/normal_estimator.cpp
index 7c6cc6d..71ad3b5 100644
--- a/hand_control/src/normal_estimator.cpp
+++ b/hand_control/src/normal_estimator.cpp
@@ -2,12 +2,10 @@
#include
#include
#include
-#include
+#include
typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud PointCloud;
-typedef Eigen::Vector4f PCLCoord; // vecteur, accès par v(0), v(1)...
-typedef geometry_msgs::Quaternion ROSCoord; // struct : x, y, z, w
class Callback {
public:
@@ -15,16 +13,15 @@ class Callback {
operator()(const PointCloud::ConstPtr& msg)
{
ROS_INFO("PointCloud received");
- PCLCoord pcl_coord();
+ Eigen::Vector4f pcl_coord();
float curvature;
std::vector indices();
// TODO : choisir tous les indices
estimator.computePointNormal(*msg, indices,
- PCLCoord, curvature);
- /* TODO
- if (curvature < ?
- publisher.publish(to_ROSCoord(pcl_coord));
- */
+ pcl_coord, curvature);
+ // publication
+ ROS_INFO("Plan published")
+ publisher.publish(to_Plan(pcl_coord, curvature));
}
Callback(ros::Publisher& pub) : publisher(pub), estimator() {}
@@ -32,17 +29,18 @@ class Callback {
private:
ros::Publisher publisher;
- pcl::NormalEstimationOMP estimator;
+ pcl::NormalEstimationOMP estimator;
- const ROSCoord::ConstPtr
- to_ROSCoord(const PCLCoord& pcl_coord)
+ const hand_control::Plan::ConstPtr
+ to_Plan(const Eigen::Vector4f& pcl_coord, const float& curvature)
{
- ROSCoord::Ptr ros_coord(new ROSCoord());
- ros_coord->x = pcl_coord(0); // a
- ros_coord->y = pcl_coord(1); // b
- ros_coord->z = pcl_coord(2); // c
- ros_coord->w = pcl_coord(3); // d
- return ros_coord;
+ hand_control::Plan::Ptr ros_msg(new 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
+ ros_msg->altitude = pcl_coord(3); // d
+ ros_msg->curvature = curvature; // \lambda
+ return ros_msg;
}
};
@@ -53,8 +51,9 @@ main(int argc, char** argv)
ros::NodeHandle node("estimator");
// initialisation
- ros::Publisher publisher = node.advertise("output", 1);
+ ros::Publisher publisher = node.advertise("output", 1);
Callback callback(publisher);
+
ros::Subscriber subscriber = node.subscribe("input", 1, callback);
// démarrage