PointCloudProvider.h
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 
25 #pragma once
26 
31 
32 #include <VisionX/interface/core/DataTypes.h>
33 #include <VisionX/interface/core/PointCloudProcessorInterface.h>
34 #include <VisionX/interface/core/PointCloudProviderInterface.h>
36 
37 #include "PointCloudConversions.h"
38 
39 namespace visionx
40 {
41 
43  {
44  public:
47  {
48  defineOptionalProperty<std::string>("frameName", "", "name of the source");
49  }
50  };
51 
52  // ====================================================================== //
53  // == class PointCloudProvider declaration ============================== //
54  // ====================================================================== //
55  /**
56  * PointCloudProvider abstract class defines a component which provide point clouds via ice or shared memory.
57  */
59  virtual public armarx::Component,
60  virtual public PointCloudProviderInterface
61  {
62 
63  public:
64  // ================================================================== //
65  // == PointCloudProvider ice interface ============================== //
66  // ================================================================== //
67 
68  /**
69  * Retrieve point clouds via Ice. Bypasses the IcesharedMemoryProvider
70  */
71  armarx::Blob getPointCloud(MetaPointCloudFormatPtr& info,
72  const Ice::Current& c = Ice::emptyCurrent) override;
73 
74  /**
75  * Returns the point cloud format info struct via Ice
76  */
77  MetaPointCloudFormatPtr
78  getPointCloudFormat(const Ice::Current& c = Ice::emptyCurrent) override;
79 
80  bool
81  hasSharedMemorySupport(const Ice::Current& c = Ice::emptyCurrent) override
82  {
83  return true;
84  }
85 
86  void enablePointCloudVisualization(bool enable,
87  bool preferRGB,
88  const Ice::Current& c = Ice::emptyCurrent);
89 
90  protected:
91  // ================================================================== //
92  // == Interface of PointCloudProvider =============================== //
93  // ================================================================== //
94  /**
95  * This is called when the Component::onInitComponent() is called.
96  *
97  * Implement this method in the derived PointCloudProvider in order to setup its parameters.
98  * Use this to set the point cloud format.
99  */
100  virtual void onInitPointCloudProvider() = 0;
101 
102  /**
103  * This is called when the Component::onConnectComponent() setup is called
104  */
105  virtual void
107  {
108  }
109 
110  /**
111  * This is called when the Component::onExitComponent() setup is called
112  *
113  * Implement this method in the derived PointCloudProvider in order clean up right before terminating.
114  */
115  virtual void onExitPointCloudProvider() = 0;
116 
117  /**
118  * offer the new point cloud.
119  */
120  template <typename PointCloudPtrT>
121  void
122  providePointCloud(PointCloudPtrT pointCloudPtr)
123  {
124  using PointCloudT = typename PointCloudPtrT::element_type;
125  using PointT = typename PointCloudT::PointType;
126 
127  if (!sharedMemoryProvider)
128  {
129  ARMARX_WARNING << "Shared memory provider is null (possibly shutting down)";
130  return;
131  }
132  MetaPointCloudFormatPtr info(MetaPointCloudFormatPtr::dynamicCast(
133  sharedMemoryProvider->getMetaInfo()->ice_clone()));
135  info->type = visionx::tools::getPointContentType<PointT>();
136  info->size = pointCloudPtr->width * pointCloudPtr->height *
138 
139  if (info->size > info->capacity)
140  {
141  const size_t maxPoints =
142  info->capacity / visionx::tools::getBytesPerPoint(info->type);
144  << "Exceeding the capacity limit of supported points (" << (maxPoints)
145  << " points). The provided point cloud contains "
146  << pointCloudPtr->width * pointCloudPtr->height
147  << " points. Cropping the point cloud to fit into limit.";
148 
149  pointCloudPtr->width = maxPoints;
150  pointCloudPtr->height = 1;
151  pointCloudPtr->resize(maxPoints);
152  info->size = maxPoints * visionx::tools::getBytesPerPoint(info->type);
153  }
154 
155  info->width = pointCloudPtr->width;
156  info->height = pointCloudPtr->height;
157 
158  if (pointCloudPtr->header.stamp)
159  {
160  info->timeProvided = pointCloudPtr->header.stamp;
161  }
162  else
163  {
164  info->timeProvided = armarx::TimeUtil::GetTime().toMicroSeconds();
165  }
166 
167  if (pointCloudPtr->header.seq)
168  {
169  info->seq = pointCloudPtr->header.seq;
170  }
171  else
172  {
173  info->seq = ++seq;
174  }
175 
176  // if (!pointCloudPtr->header.frame_id.empty())
177  // {
178  // info->frameId = pointCloudPtr->header.frame_id;
179  // }
180  ARMARX_CHECK_GREATER_EQUAL((int)intermediateBuffer.size(), info->capacity);
181  ARMARX_CHECK_GREATER_EQUAL((int)intermediateBuffer.size(), info->size);
182  ARMARX_CHECK_GREATER(intermediateBuffer.size(), 0);
183  auto buf = intermediateBuffer.data();
185  visionx::tools::convertFromPCL(pointCloudPtr, (void**)&buf);
186  {
188  sharedMemoryProvider->getScopedWriteLock());
189 
190  sharedMemoryProvider->setMetaInfo(info, false);
191 
192  unsigned char* buffer = sharedMemoryProvider->getBuffer();
193 
194  memcpy(buffer, intermediateBuffer.data(), info->size);
195  }
197  // notify processors
198  pointCloudProcessorProxy->reportPointCloudAvailable(getName());
199  }
200 
201  // ================================================================== //
202  // == Component implementation ====================================== //
203  // ================================================================== //
204  /**
205  * @see Component::onInitComponent()
206  */
207  void onInitComponent() override;
208 
209  /**
210  * @see Component::onConnectComponent()
211  */
212  void onConnectComponent() override;
213 
214  void onDisconnectComponent() override;
215 
216  /**
217  * @see Component::onExitComponent()
218  */
219  void onExitComponent() override;
220 
221  protected:
222  /**
223  * Retrieve whether provider is exiting
224  *
225  * @return exiting status
226  */
227  bool
228  isExiting() const
229  {
230  return exiting;
231  }
232 
233  void updateComponentMetaInfo(const MetaPointCloudFormatPtr& info);
234 
235  /**
236  * Ice proxy of the point cloud processor interface
237  */
238  PointCloudProcessorInterfacePrx pointCloudProcessorProxy;
239 
240  /**
241  * default point cloud format used to initialize shared memory
242  */
243  virtual MetaPointCloudFormatPtr getDefaultPointCloudFormat();
244 
245  private:
246  /**
247  * shared memory provider
248  */
250  sharedMemoryProvider;
251 
252  /**
253  * Point cloud information
254  */
255  MetaPointCloudFormatPtr pointCloudFormat;
256 
257  /**
258  * Indicates that exit is in process and thread needs to be stopped
259  */
260  bool exiting;
261 
262  int seq;
263  FPSCounter fps;
264 
265  std::vector<unsigned char> intermediateBuffer;
266  };
267 } // namespace visionx
visionx::PointCloudProvider::updateComponentMetaInfo
void updateComponentMetaInfo(const MetaPointCloudFormatPtr &info)
Definition: PointCloudProvider.cpp:118
armarx::IceSharedMemoryProvider::getBuffer
MemoryObject * getBuffer()
Retrieve pointer to buffer.
Definition: IceSharedMemoryProvider.h:144
visionx::PointCloudProvider::onConnectComponent
void onConnectComponent() override
Definition: PointCloudProvider.cpp:77
visionx::armem::pointcloud::PointType
PointType
Definition: constants.h:78
armarx::IceSharedMemoryProvider::getScopedWriteLock
SharedMemoryScopedWriteLockPtr getScopedWriteLock() const
Retrieve scoped lock for writing to the memory.
Definition: IceSharedMemoryProvider.h:156
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
ARMARX_CHECK_GREATER
#define ARMARX_CHECK_GREATER(lhs, rhs)
This macro evaluates whether lhs is greater (>) than rhs and if it turns out to be false it will thro...
Definition: ExpressionException.h:116
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
visionx::PointCloudProvider::hasSharedMemorySupport
bool hasSharedMemorySupport(const Ice::Current &c=Ice::emptyCurrent) override
Definition: PointCloudProvider.h:81
armarx::IceSharedMemoryProvider::setMetaInfo
void setMetaInfo(const typename MemoryObjectMetaInfo::PointerType &info, bool threadSafe=true)
Definition: IceSharedMemoryProvider.h:228
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::SharedMemoryScopedWriteLockPtr
std::shared_ptr< SharedMemoryScopedWriteLock > SharedMemoryScopedWriteLockPtr
Definition: SharedMemoryProvider.h:46
visionx::PointCloudProvider::getPointCloud
armarx::Blob getPointCloud(MetaPointCloudFormatPtr &info, const Ice::Current &c=Ice::emptyCurrent) override
Retrieve point clouds via Ice.
Definition: PointCloudProvider.cpp:37
armarx::IceSharedMemoryProvider::getMetaInfo
MemoryObjectMetaInfo::PointerType getMetaInfo(bool threadSafe=true) const
getMetaInfo returns a copy of the memory object information
Definition: IceSharedMemoryProvider.h:243
visionx::PointCloudProvider::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: PointCloudProvider.cpp:95
visionx::PointCloudProvider::onExitComponent
void onExitComponent() override
Definition: PointCloudProvider.cpp:106
IceSharedMemoryProvider.h
visionx::tools::convertFromPCL
void convertFromPCL(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr sourcePtr, void **target)
Definition: PointCloudConversions.cpp:121
visionx::PointCloudProviderPropertyDefinitions::PointCloudProviderPropertyDefinitions
PointCloudProviderPropertyDefinitions(std::string prefix)
Definition: PointCloudProvider.h:45
visionx::PointCloudProvider::getPointCloudFormat
MetaPointCloudFormatPtr getPointCloudFormat(const Ice::Current &c=Ice::emptyCurrent) override
Returns the point cloud format info struct via Ice.
Definition: PointCloudProvider.cpp:46
visionx::PointCloudProviderPropertyDefinitions
Definition: PointCloudProvider.h:42
visionx::PointCloudProvider::pointCloudProcessorProxy
PointCloudProcessorInterfacePrx pointCloudProcessorProxy
Ice proxy of the point cloud processor interface.
Definition: PointCloudProvider.h:238
visionx::PointCloudProvider::onInitComponent
void onInitComponent() override
Definition: PointCloudProvider.cpp:59
armarx::PointCloudT
pcl::PointCloud< PointT > PointCloudT
Definition: Common.h:30
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition: ExpressionException.h:123
Component.h
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:95
visionx::PointCloudProvider::getDefaultPointCloudFormat
virtual MetaPointCloudFormatPtr getDefaultPointCloudFormat()
default point cloud format used to initialize shared memory
Definition: PointCloudProvider.cpp:132
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
ExpressionException.h
visionx::PointCloudProvider::enablePointCloudVisualization
void enablePointCloudVisualization(bool enable, bool preferRGB, const Ice::Current &c=Ice::emptyCurrent)
PointCloudConversions.h
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
visionx::PointCloudProvider::providePointCloud
void providePointCloud(PointCloudPtrT pointCloudPtr)
offer the new point cloud.
Definition: PointCloudProvider.h:122
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
TimeUtil.h
IceUtil::Handle
Definition: forward_declarations.h:29
armarx::ComponentPropertyDefinitions::ComponentPropertyDefinitions
ComponentPropertyDefinitions(std::string prefix, bool hasObjectNameParameter=true)
Definition: Component.cpp:37
visionx::PointCloudProvider::onInitPointCloudProvider
virtual void onInitPointCloudProvider()=0
This is called when the Component::onInitComponent() is called.
visionx::tools::getBytesPerPoint
size_t getBytesPerPoint(visionx::PointContentType pointContent)
Definition: PointCloudConversions.cpp:64
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:92
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:107
visionx::PointCloudProvider::isExiting
bool isExiting() const
Retrieve whether provider is exiting.
Definition: PointCloudProvider.h:228
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
FPSCounter.h
visionx::PointCloudProvider::onExitPointCloudProvider
virtual void onExitPointCloudProvider()=0
This is called when the Component::onExitComponent() setup is called.
visionx::PointCloudProvider
PointCloudProvider abstract class defines a component which provide point clouds via ice or shared me...
Definition: PointCloudProvider.h:58
visionx::PointCloudProvider::onConnectPointCloudProvider
virtual void onConnectPointCloudProvider()
This is called when the Component::onConnectComponent() setup is called.
Definition: PointCloudProvider.h:106
visionx::FPSCounter
The FPSCounter class provides methods for calculating the frames per second (FPS) count in periodic t...
Definition: FPSCounter.h:36
visionx::PointT
pcl::PointXYZRGBA PointT
Definition: MaskRCNNPointCloudObjectLocalizer.h:79