ajout message Plan
This commit is contained in:
parent
399a1854b7
commit
d45cde8b6d
4 changed files with 38 additions and 29 deletions
|
@ -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
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
3
hand_control/msg/Plan.msg
Normal file
3
hand_control/msg/Plan.msg
Normal file
|
@ -0,0 +1,3 @@
|
||||||
|
geometry_msgs/Point normal
|
||||||
|
float64 altitude
|
||||||
|
float64 curvature
|
|
@ -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>
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in a new issue