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;
RobotinoIrPclThread()
Constructor.
void remove_pointcloud(const char *id)
Remove the point cloud.
A class for handling time.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
float * distance() const
Get distance value.
RobotinoSensorInterface Fawkes BlackBoard Interface.
Thread aspect to use blocked timing.
const Time * timestamp() const
Get timestamp of last write.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
void read()
Read from BlackBoard into local copy.
virtual void finalize()
Finalize the thread.
bool changed() const
Check if data has been changed.
virtual void loop()
Code to execute in the thread.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
size_t maxlenof_distance() const
Get maximum length of distance value.
virtual void init()
Initialize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.