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
29using 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
45void
47{
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
70void
72{
73 pcl_manager->remove_pointcloud("robotino-ir");
74 blackboard->close(sens_if_);
75
76 delete[] angle_sines_;
77 delete[] angle_cosines_;
78}
79
80void
82{
83 // update sensor values in interface
84 sens_if_->read();
85
86 if (sens_if_->refreshed()) {
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}
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.
Definition: blackboard.h:44
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.
Definition: configurable.h:41
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.
Definition: interface.cpp:714
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool refreshed() const
Check if data has been refreshed.
Definition: interface.cpp:811
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
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.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Fawkes library namespace.