23 #include "ir_pcl_thread.h"
25 #include <interfaces/RobotinoSensorInterface.h>
28 using namespace fawkes;
39 :
Thread(
"RobotinoIrPclThread",
Thread::OPMODE_WAITFORWAKEUP),
53 pcl_xyz_->is_dense =
false;
56 pcl_xyz_->points.resize(pcl_xyz_->width * pcl_xyz_->height);
57 pcl_xyz_->header.frame_id =
"/base_link";
61 float angle_offset = (2 * M_PI) / pcl_xyz_->width;
62 angle_sines_ =
new float[pcl_xyz_->width];
63 angle_cosines_ =
new float[pcl_xyz_->width];
64 for (
unsigned int i = 0; i < pcl_xyz_->width; ++i) {
65 angle_sines_[i] = sinf(angle_offset * i);
66 angle_cosines_[i] = cosf(angle_offset * i);
78 delete angle_cosines_;
89 const float *distances = sens_if_->
distance();
94 pcl_utils::set_time(pcl_xyz_, *ct);
96 for (
unsigned int i = 0; i < pcl_xyz_->width; ++i) {
97 if (distances[i] == 0.) {
98 pcl.points[i].x = pcl.points[i].y = pcl.points[i].z =
99 std::numeric_limits<float>::quiet_NaN();
101 pcl.points[i].x = (distances[i] + 0.2) * angle_cosines_[i];
102 pcl.points[i].y = (distances[i] + 0.2) * angle_sines_[i];
103 pcl.points[i].z = 0.025;