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