ajout message Plan

This commit is contained in:
Louis-Guillaume DUBOIS 2015-04-23 17:58:31 +02:00
parent 399a1854b7
commit d45cde8b6d
4 changed files with 38 additions and 29 deletions

View file

@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs std_msgs
freenect_camera freenect_camera
freenect_launch freenect_launch
message_generation
) )
## System dependencies are found with CMake's conventions ## 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 ...) ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder ## Generate messages in the 'msg' folder
# add_message_files( add_message_files(
# FILES FILES
# Message1.msg Plan.msg
# Message2.msg )
# )
## Generate services in the 'srv' folder ## Generate services in the 'srv' folder
# add_service_files( # add_service_files(
@ -70,10 +70,11 @@ find_package(catkin REQUIRED COMPONENTS
# ) # )
## Generate added messages and services with any dependencies listed here ## Generate added messages and services with any dependencies listed here
# generate_messages( generate_messages(
# DEPENDENCIES DEPENDENCIES
# std_msgs std_msgs
# ) geometry_msgs
)
################################### ###################################
## catkin specific configuration ## ## catkin specific configuration ##
@ -87,7 +88,7 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package( catkin_package(
# INCLUDE_DIRS include # INCLUDE_DIRS include
# LIBRARIES hand_control # 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 # DEPENDS system_lib
) )

View file

@ -0,0 +1,3 @@
geometry_msgs/Point normal
float64 altitude
float64 curvature

View file

@ -26,6 +26,11 @@
<build_depend>pcl_conversions</build_depend> <build_depend>pcl_conversions</build_depend>
<build_depend>roscpp</build_depend> <build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend> <build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>freenect_stack</build_depend>
<build_depend>perception_pcl</build_depend>
<run_depend>perception_pcl</run_depend>
<run_depend>freenect_stack</run_depend>
<run_depend>ardrone_autonomy</run_depend> <run_depend>ardrone_autonomy</run_depend>
<run_depend>freenect_camera</run_depend> <run_depend>freenect_camera</run_depend>
<run_depend>freenect_launch</run_depend> <run_depend>freenect_launch</run_depend>
@ -35,6 +40,7 @@
<run_depend>pcl_conversions</run_depend> <run_depend>pcl_conversions</run_depend>
<run_depend>roscpp</run_depend> <run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend> <run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->
<export> <export>

View file

@ -2,12 +2,10 @@
#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.h>
#include <geometry_msgs/Quaternion.h> #include <hand_control/Plan.h>
typedef pcl::PointXYZRGB Point; typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud; typedef pcl::PointCloud<Point> 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 { class Callback {
public: public:
@ -15,16 +13,15 @@ class Callback {
operator()(const PointCloud::ConstPtr& msg) operator()(const PointCloud::ConstPtr& msg)
{ {
ROS_INFO("PointCloud received"); ROS_INFO("PointCloud received");
PCLCoord pcl_coord(); Eigen::Vector4f pcl_coord();
float curvature; float curvature;
std::vector<int> indices(); std::vector<int> indices();
// TODO : choisir tous les indices // TODO : choisir tous les indices
estimator.computePointNormal(*msg, indices, estimator.computePointNormal(*msg, indices,
PCLCoord, curvature); pcl_coord, curvature);
/* TODO // publication
if (curvature < ? ROS_INFO("Plan published")
publisher.publish(to_ROSCoord(pcl_coord)); publisher.publish(to_Plan(pcl_coord, curvature));
*/
} }
Callback(ros::Publisher& pub) : publisher(pub), estimator() {} Callback(ros::Publisher& pub) : publisher(pub), estimator() {}
@ -32,17 +29,18 @@ class Callback {
private: private:
ros::Publisher publisher; ros::Publisher publisher;
pcl::NormalEstimationOMP<Point, pcl::Normal> estimator; pcl::NormalEstimationOMP<Point, Eigen::Vector4f> estimator;
const ROSCoord::ConstPtr const hand_control::Plan::ConstPtr
to_ROSCoord(const PCLCoord& pcl_coord) to_Plan(const Eigen::Vector4f& pcl_coord, const float& curvature)
{ {
ROSCoord::Ptr ros_coord(new ROSCoord()); hand_control::Plan::Ptr ros_msg(new Plan());
ros_coord->x = pcl_coord(0); // a ros_msg->normal.x = pcl_coord(0); // a
ros_coord->y = pcl_coord(1); // b ros_msg->normal.y = pcl_coord(1); // b
ros_coord->z = pcl_coord(2); // c ros_msg->normal.z = pcl_coord(2); // c
ros_coord->w = pcl_coord(3); // d ros_msg->altitude = pcl_coord(3); // d
return ros_coord; ros_msg->curvature = curvature; // \lambda
return ros_msg;
} }
}; };
@ -53,8 +51,9 @@ main(int argc, char** argv)
ros::NodeHandle node("estimator"); ros::NodeHandle node("estimator");
// initialisation // initialisation
ros::Publisher publisher = node.advertise<ROSCoord>("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