23 #include "ir_pcl_thread.h"
25 #include <interfaces/RobotinoSensorInterface.h>
40 :
Thread(
"RobotinoIrPclThread",
Thread::OPMODE_WAITFORWAKEUP),
53 pcl_xyz_->is_dense =
false;
56 pcl_xyz_->points.resize((
size_t)pcl_xyz_->width * (
size_t)pcl_xyz_->height);
57 pcl_xyz_->header.frame_id =
config->
get_string(
"/hardware/robotino/base_frame");
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);
76 delete[] angle_sines_;
77 delete[] angle_cosines_;
88 const float *distances = sens_if_->
distance();
93 pcl_utils::set_time(pcl_xyz_, *ct);
95 for (
unsigned int i = 0; i < pcl_xyz_->width; ++i) {
96 if (distances[i] == 0.) {
97 pcl.points[i].x = pcl.points[i].y = pcl.points[i].z =
98 std::numeric_limits<float>::quiet_NaN();
100 pcl.points[i].x = (distances[i] + 0.2) * angle_cosines_[i];
101 pcl.points[i].y = (distances[i] + 0.2) * angle_sines_[i];
102 pcl.points[i].z = 0.025;