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