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
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) :
publisher(pub), estimator(), number(0) {}
publisher(pub), estimator() {}
private:
ros::Publisher publisher;
pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
uint32_t number;
inline
const hand_control::Plan::ConstPtr
to_Plan(const float& x, const float& y,
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());
ros_msg->normal.x = x;
@ -57,7 +56,7 @@ class Callback {
uint64_t nsec64 = (msec64 % 1000000) * 1000;
ros_msg->header.stamp.sec = (uint32_t) sec64;
ros_msg->header.stamp.nsec = (uint32_t) nsec64;
ros_msg->header.seq = number++;
ros_msg->header.seq = seq;
ros_msg->header.frame_id = "0";
return ros_msg;
}

8
src/random_pcl_publisher.cpp
View file

@ -1,4 +1,5 @@
#include <ros/ros.h>
#include <time.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
// for UniformGenerator
@ -13,7 +14,7 @@ class Generator
{
public:
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);
cgen.setParameters(params);
@ -30,6 +31,10 @@ class Generator
pcl->points[i].g = (uint8_t) 255;
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;
}
@ -37,6 +42,7 @@ class Generator
pcl::common::CloudGenerator<pcl::PointXYZRGB, UGenerator> cgen;
int length;
double min, max;
uint32_t number;
};
int