ajout message Plan
This commit is contained in:
Louis-Guillaume DUBOIS
parent
399a1854b7
commit
d45cde8b6d
4 changed files with 38 additions and 29 deletions
|
|
@ -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
|
||||
)
|
||||
|
||||
|
|
|
|||
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>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>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in a new issue