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 
27 #include <limits>
28 
29 using namespace fawkes;
30 
31 /** @class RobotinoIrPclThread "ir_pcl_thread.h"
32  * Robotino IR distances as point cloud.
33  * This thread integrates into the Fawkes main loop at the SENSOR_PROCESS
34  * hook and converts sensor data to a pointcloud
35  * @author Tim Niemueller
36  */
37 
38 /** Constructor. */
40 : Thread("RobotinoIrPclThread", Thread::OPMODE_WAITFORWAKEUP),
41  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
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((size_t)pcl_xyz_->width * (size_t)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 void
72 {
73  pcl_manager->remove_pointcloud("robotino-ir");
74  blackboard->close(sens_if_);
75 
76  delete[] angle_sines_;
77  delete[] angle_cosines_;
78 }
79 
80 void
82 {
83  // update sensor values in interface
84  sens_if_->read();
85 
86  if (sens_if_->changed()) {
87  const Time * ct = sens_if_->timestamp();
88  const float *distances = sens_if_->distance();
89 
90  pcl::PointCloud<pcl::PointXYZ> &pcl = **pcl_xyz_;
91  pcl.header.seq += 1;
92 
93  pcl_utils::set_time(pcl_xyz_, *ct);
94 
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();
99  } else {
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; // 2.5 cm above ground
103  }
104  }
105  }
106 }
RobotinoIrPclThread::init
virtual void init()
Initialize the thread.
Definition: ir_pcl_thread.cpp:46
fawkes::Interface::timestamp
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:705
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
fawkes::PointCloudManager::remove_pointcloud
void remove_pointcloud(const char *id)
Remove the point cloud.
Definition: pointcloud_manager.cpp:66
RobotinoIrPclThread::RobotinoIrPclThread
RobotinoIrPclThread()
Constructor.
Definition: ir_pcl_thread.cpp:39
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::RobotinoSensorInterface::maxlenof_distance
size_t maxlenof_distance() const
Get maximum length of distance value.
Definition: RobotinoSensorInterface.cpp:297
RobotinoIrPclThread::finalize
virtual void finalize()
Finalize the thread.
Definition: ir_pcl_thread.cpp:71
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes
Fawkes library namespace.
fawkes::Interface::changed
bool changed() const
Check if data has been changed.
Definition: interface.cpp:780
fawkes::RobotinoSensorInterface
RobotinoSensorInterface Fawkes BlackBoard Interface.
Definition: RobotinoSensorInterface.h:34
pcl::PointCloud< pcl::PointXYZ >
RobotinoIrPclThread::loop
virtual void loop()
Code to execute in the thread.
Definition: ir_pcl_thread.cpp:81
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::RobotinoSensorInterface::distance
float * distance() const
Get distance value.
Definition: RobotinoSensorInterface.cpp:272
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::PointCloudManager::add_pointcloud
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT >> cloud)
Add point cloud.
Definition: pointcloud_manager.h:78
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
fawkes::PointCloudAspect::pcl_manager
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47