Fawkes API  Fawkes Development Version
ir_pcl_thread.cpp
1 
2 /***************************************************************************
3  * ir_pcl_thread.cpp - Robotino IR point cloud thread
4  *
5  * Created: Mon Mar 26 14:06:29 2012
6  * Copyright 2011-2012 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "ir_pcl_thread.h"
24 
25 #include <interfaces/RobotinoSensorInterface.h>
26 #include <limits>
27 
28 using namespace fawkes;
29 
30 /** @class RobotinoIrPclThread "ir_pcl_thread.h"
31  * Robotino IR distances as point cloud.
32  * This thread integrates into the Fawkes main loop at the SENSOR_PROCESS
33  * hook and converts sensor data to a pointcloud
34  * @author Tim Niemueller
35  */
36 
37 /** Constructor. */
39  : Thread("RobotinoIrPclThread", Thread::OPMODE_WAITFORWAKEUP),
40  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
41 {
42 }
43 
44 
45 void
47 {
48  sens_if_ = blackboard->open_for_reading<RobotinoSensorInterface>("Robotino");
49 
50  sens_if_->read();
51 
52  pcl_xyz_ = new pcl::PointCloud<pcl::PointXYZ>();
53  pcl_xyz_->is_dense = false;
54  pcl_xyz_->width = sens_if_->maxlenof_distance();
55  pcl_xyz_->height = 1;
56  pcl_xyz_->points.resize(pcl_xyz_->width * pcl_xyz_->height);
57  pcl_xyz_->header.frame_id = config->get_string("/hardware/robotino/base_frame");
58 
59  pcl_manager->add_pointcloud("robotino-ir", pcl_xyz_);
60 
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);
67  }
68 }
69 
70 
71 void
73 {
74  pcl_manager->remove_pointcloud("robotino-ir");
75  blackboard->close(sens_if_);
76 
77  delete angle_sines_;
78  delete angle_cosines_;
79 }
80 
81 void
83 {
84  // update sensor values in interface
85  sens_if_->read();
86 
87  if (sens_if_->changed()) {
88  const Time *ct = sens_if_->timestamp();
89  const float *distances = sens_if_->distance();
90 
91  pcl::PointCloud<pcl::PointXYZ> &pcl = **pcl_xyz_;
92  pcl.header.seq += 1;
93 
94  pcl_utils::set_time(pcl_xyz_, *ct);
95 
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();
100  } else {
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; // 2.5 cm above ground
104  }
105  }
106  }
107 }
RobotinoIrPclThread()
Constructor.
void remove_pointcloud(const char *id)
Remove the point cloud.
Definition: pointcloud.h:30
Fawkes library namespace.
A class for handling time.
Definition: time.h:91
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
Definition: thread.h:42
RobotinoSensorInterface Fawkes BlackBoard Interface.
Thread aspect to use blocked timing.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:50
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
virtual void finalize()
Finalize the thread.
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:718
bool changed() const
Check if data has been changed.
Definition: interface.cpp:796
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.
Definition: configurable.h:44
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.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.