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
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
)

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>roscpp</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>freenect_camera</run_depend>
<run_depend>freenect_launch</run_depend>
@ -35,6 +40,7 @@
<run_depend>pcl_conversions</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>

View file

@ -2,12 +2,10 @@
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <geometry_msgs/Quaternion.h>
#include <hand_control/Plan.h>
typedef pcl::PointXYZRGB Point;
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 {
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<int> 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<Point, pcl::Normal> estimator;
pcl::NormalEstimationOMP<Point, Eigen::Vector4f> 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<ROSCoord>("output", 1);
ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1);
Callback callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
// démarrage