PointCloudProvider.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package VisionX::Core
19  * @author David Gonzalez Aguirre (david dot gonzalez at kit dot edu)
20  * @date 2014
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
27 #include "PointCloudProvider.h"
28 
29 #include <string>
30 
31 using namespace armarx;
32 namespace visionx
33 {
34  // ================================================================== //
35  // == PointCloudProvider ice interface ============================== //
36  // ================================================================== //
37  armarx::Blob PointCloudProvider::getPointCloud(MetaPointCloudFormatPtr& info, const Ice::Current& c)
38  {
39  MetaInfoSizeBasePtr infoBase;
40  armarx::Blob result = sharedMemoryProvider->getData(infoBase);
41  // info is an output parameter and must be set explicitly
42  info = MetaPointCloudFormatPtr::dynamicCast(infoBase);
43  return result;
44  }
45 
46  MetaPointCloudFormatPtr PointCloudProvider::getPointCloudFormat(const Ice::Current& c)
47  {
48  // armarx::SharedMemoryScopedWriteLockPtr lock(sharedMemoryProvider->getScopedWriteLock());
49 
50  return sharedMemoryProvider->getMetaInfo();
51  }
52 
53 
54 
55 
56  // ================================================================== //
57  // == Component implementation ====================================== //
58  // ================================================================== //
59  void PointCloudProvider::onInitComponent()
60  {
62  // init members
63  exiting = false;
64 
65  // call setup of derived poitcloud provider implementation to setup Point Clouds type and size
66  onInitPointCloudProvider();
67 
68  MetaPointCloudFormatPtr info = getDefaultPointCloudFormat();
69  intermediateBuffer.resize(info->capacity);
70  sharedMemoryProvider = new IceSharedMemoryProvider<unsigned char, MetaPointCloudFormat>(this, info, "PointCloudProvider");
71 
72  // Offer topic for point cloud events
73  offeringTopic(getName() + ".PointCloudListener");
74  }
75 
76 
77  void PointCloudProvider::onConnectComponent()
78  {
80  pointCloudProcessorProxy = getTopic<PointCloudProcessorInterfacePrx>(getName() + ".PointCloudListener");
81 
82  if (!sharedMemoryProvider)
83  {
84  // Ensure that the shared memory provider is created
85  ARMARX_INFO << "Shared memory provider is null: Recreating it";
86  MetaPointCloudFormatPtr info = getDefaultPointCloudFormat();
87  sharedMemoryProvider = new IceSharedMemoryProvider<unsigned char, MetaPointCloudFormat>(this, info, "PointCloudProvider");
88  }
89  sharedMemoryProvider->start();
90 
91  onConnectPointCloudProvider();
92  fps.reset();
93  }
94 
95  void PointCloudProvider::onDisconnectComponent()
96  {
98  if (sharedMemoryProvider)
99  {
100  sharedMemoryProvider->stop();
101  sharedMemoryProvider = nullptr;
102  }
103  }
104 
105 
106  void PointCloudProvider::onExitComponent()
107  {
108  ARMARX_TRACE;
109  if (sharedMemoryProvider)
110  {
111  sharedMemoryProvider->stop();
112  sharedMemoryProvider = nullptr;
113  }
114  exiting = true;
115  onExitPointCloudProvider();
116  }
117 
118  void PointCloudProvider::updateComponentMetaInfo(const MetaPointCloudFormatPtr& info)
119  {
120  fps.update();
121  setMetaInfo("PointCloudTimestamp", new armarx::Variant(IceUtil::Time::microSeconds(info->timeProvided).toDateTime()));
122  setMetaInfo("FPS", new armarx::Variant(fps.getFPS()));
123  setMetaInfo("MaxCycleTimeMS", new armarx::Variant(fps.getMaxCycleTimeMS()));
124  setMetaInfo("MeanCycleTimeMS", new armarx::Variant(fps.getMeanCycleTimeMS()));
125  setMetaInfo("MinCycleTimeMS", new armarx::Variant(fps.getMinCycleTimeMS()));
126  setMetaInfo("PointCloudSize", new armarx::Variant(info->size));
127 
128  }
129 
130 
131 
132  MetaPointCloudFormatPtr PointCloudProvider::getDefaultPointCloudFormat()
133  {
134  ARMARX_TRACE;
135  MetaPointCloudFormatPtr info = new MetaPointCloudFormat();
136  //info->frameId = getProperty<std::string>("frameId").getValue();
137  info->type = PointContentType::eColoredPoints;
138  info->capacity = 640 * 480 * sizeof(ColoredPoint3D);// + info->frameId.size();
139  info->size = info->capacity;
140  return info;
141  }
142 }
143 
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
trace.h
PointCloudProvider.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::IceSharedMemoryProvider< unsigned char, MetaPointCloudFormat >
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
Variant.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28