LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

better numbering in normal_estimator

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-02 18:28:40 +02:00
parent cae0e41b7c
commit 454b0acd65
2 changed files with 11 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

9
src/normal_estimator.cpp
View file

@ -29,22 +29,21 @@ class Callback {
// publication // publication
ROS_INFO("Plan published"); ROS_INFO("Plan published");
publisher.publish(to_Plan(x, y, z, h, c, msg->header.stamp)); publisher.publish(to_Plan(x, y, z, h, c, msg->header.seq, msg->header.stamp));
} }
Callback(ros::Publisher& pub) : Callback(ros::Publisher& pub) :
publisher(pub), estimator(), number(0) {} publisher(pub), estimator() {}
private: private:
ros::Publisher publisher; ros::Publisher publisher;
pcl::NormalEstimationOMP<Point, pcl::Normal> estimator; pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
uint32_t number;
inline inline
const hand_control::Plan::ConstPtr const hand_control::Plan::ConstPtr
to_Plan(const float& x, const float& y, to_Plan(const float& x, const float& y,
const float& z, const float& h, const float& z, const float& h,
const float& c, uint64_t msec64) const float& c, uint32_t seq, uint64_t msec64)
{ {
hand_control::Plan::Ptr ros_msg(new hand_control::Plan()); hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
ros_msg->normal.x = x; ros_msg->normal.x = x;
@ -57,7 +56,7 @@ class Callback {
uint64_t nsec64 = (msec64 % 1000000) * 1000; uint64_t nsec64 = (msec64 % 1000000) * 1000;
ros_msg->header.stamp.sec = (uint32_t) sec64; ros_msg->header.stamp.sec = (uint32_t) sec64;
ros_msg->header.stamp.nsec = (uint32_t) nsec64; ros_msg->header.stamp.nsec = (uint32_t) nsec64;
ros_msg->header.seq = number++; ros_msg->header.seq = seq;
ros_msg->header.frame_id = "0"; ros_msg->header.frame_id = "0";
return ros_msg; return ros_msg;
} }

8
src/random_pcl_publisher.cpp
View file

@ -1,4 +1,5 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <time.h>
#include <pcl_ros/point_cloud.h> #include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
// for UniformGenerator // for UniformGenerator
@ -13,7 +14,7 @@ class Generator
{ {
public: public:
Generator(int len, double m, double M) Generator(int len, double m, double M)
: length(len), min(m), max(M), cgen() : length(len), min(m), max(M), cgen(), number(0)
{ {
UGenerator::Parameters params(min, max, -1); UGenerator::Parameters params(min, max, -1);
cgen.setParameters(params); cgen.setParameters(params);
@ -30,6 +31,10 @@ class Generator
pcl->points[i].g = (uint8_t) 255; pcl->points[i].g = (uint8_t) 255;
pcl->points[i].b = (uint8_t) 0; pcl->points[i].b = (uint8_t) 0;
} }
ros::Time now = ros::Time::now();
pcl->header.stamp = now.toNSec() / 1000;
pcl->header.seq = number++;
pcl->header.frame_id = "0";
return pcl; return pcl;
} }
@ -37,6 +42,7 @@ class Generator
pcl::common::CloudGenerator<pcl::PointXYZRGB, UGenerator> cgen; pcl::common::CloudGenerator<pcl::PointXYZRGB, UGenerator> cgen;
int length; int length;
double min, max; double min, max;
uint32_t number;
}; };
int int