PointCloudToArViz.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package VisionX::ArmarXObjects::PointCloudToArViz
17  * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
18  * @date 2020
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "PointCloudToArViz.h"
24 
25 #include <pcl/common/transforms.h>
26 #include <pcl/impl/point_types.hpp>
27 #include <pcl/point_types.h>
28 
32 
36 
37 namespace visionx
38 {
39 
42  {
43  defineOptionalProperty<int>(
44  "ProviderWaitMs", 100, "Time to wait for each point cloud provider [ms].");
45  }
46 
49  {
52 
53  defs->optional(
54  params.pointSizeInPixels, "viz.pointSizeInPixels", "The point size in pixels.");
55  defs->optional(params.checkFinite,
56  "viz.checkFinite",
57  "Enable to check points for being finite. "
58  "\nEnable this option if the drawn point cloud causes the screen to become "
59  "purely white.");
60  defs->optional(params.labeled,
61  "viz.labeled",
62  "If true and point cloud is labeled, draw colors according to labels.");
63  defs->optional(params.alsoVisualizeNode,
64  "viz.showNode",
65  "If true also the node's coordinate systems is shown.");
66 
67  defs->optional(params.robotName, "robotName", "");
68  defs->optional(
69  params.pointCloudNodeName,
70  "pointCloudNodeName",
71  " If the point cloud header does not provide a frame id, this info is used.");
72 
73  return defs;
74  }
75 
77  {
78  addPlugin(virtualRobotReaderPlugin);
79  }
80 
81  std::string
83  {
84  return "PointCloudToArViz";
85  }
86 
87  void
89  {
90  params.providerWaitTime =
91  IceUtil::Time::milliSeconds(getProperty<int>("ProviderWaitMs").getValue());
92  }
93 
94  void
96  {
97  if (virtualRobotReaderPlugin->isAvailable())
98  {
99  robot = virtualRobotReaderPlugin->get().getRobot(params.robotName);
100  // No need to synchronize the robot here. We synchronize it later anyway.
101  }
102 
105  }
106 
107  void
109  {
110  }
111 
112  void
114  {
115  }
116 
117  void
119  {
120  // Fetch input clouds.
121  for (const std::string& providerName : getPointCloudProviderNames())
122  {
123  if (waitForPointClouds(providerName,
124  static_cast<int>(params.providerWaitTime.toMilliSeconds())))
125  {
126  auto it = cache.find(providerName);
127  if (it == cache.end())
128  {
129  it = cache.emplace(providerName, armarx::viz::PointCloud(providerName)).first;
130  }
131  armarx::viz::PointCloud& vizCloud = it->second;
132 
133  Parameters params;
134  {
135  std::scoped_lock lock(paramMutex);
136  params = this->params;
137  }
138 
139  vizCloud.checkFinite(params.checkFinite);
140  vizCloud.pointSizeInPixels(params.pointSizeInPixels);
141 
142  const PointContentType type = getPointCloudFormat(providerName)->type;
143  std::optional<PointCloudToArViz::OriginInfo> originInfo;
144 
145  if (visionx::tools::isLabeled(type))
146  {
147  pcl::PointCloud<pcl::PointXYZRGBL>::Ptr inputCloud(
148  new pcl::PointCloud<pcl::PointXYZRGBL>);
149  getPointClouds(providerName, inputCloud);
150  originInfo = applyCustomTransform(*inputCloud);
151  vizCloud.pointCloud(*inputCloud, params.labeled);
152  }
153  else
154  {
155  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr inputCloud(
156  new pcl::PointCloud<pcl::PointXYZRGBA>);
157  getPointClouds(providerName, inputCloud);
158  originInfo = applyCustomTransform(*inputCloud);
159  vizCloud.pointCloud(*inputCloud);
160  }
161 
162  armarx::viz::Layer layer = arviz.layer(providerName);
163  if (params.alsoVisualizeNode && originInfo.has_value())
164  {
165  auto originPose = armarx::viz::Pose(originInfo->name);
166  originPose.pose(originInfo->pose);
167  originPose.scale(3);
168  layer.add(originPose);
169  }
170 
171  layer.add(vizCloud);
172  arviz.commit({layer});
173  }
174  else
175  {
176  ARMARX_VERBOSE << "Timeout waiting for point cloud provider " << providerName
177  << ".";
178  }
179  }
180  }
181 
182  void
183  PointCloudToArViz::setPointSizeInPixels(float pointSizeInPixels, const Ice::Current&)
184  {
185  std::scoped_lock lock(paramMutex);
186  params.pointSizeInPixels = pointSizeInPixels;
187  }
188 
189  void
190  PointCloudToArViz::setCustomTransform(const armarx::PoseBasePtr& transformPtr,
191  const Ice::Current&)
192  {
193  return;
194 
195  // std::scoped_lock lock(paramMutex);
196  // params.customTransform = armarx::PosePtr::dynamicCast(transformPtr)->toEigen();
197  // if (params.customTransform->isIdentity())
198  // {
199  // params.customTransform = std::nullopt;
200  // }
201  }
202 
203  template <class PointT>
204  std::optional<PointCloudToArViz::OriginInfo>
205  PointCloudToArViz::applyCustomTransform(pcl::PointCloud<PointT>& pointCloud)
206  {
207  ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
208  if (not virtualRobotReaderPlugin->isAvailable())
209  {
211  << "The robot reader plugin is deactivated. Won't transform point cloud";
212  return std::nullopt;
213  }
214 
215  ARMARX_CHECK_NOT_NULL(robot);
216  ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(
217  *robot, armarx::core::time::Duration::MicroSeconds(pointCloud.header.stamp)));
218 
219  const auto pointCloudNodeName = [&]() -> std::string
220  {
221  // highest prio: use frame id of point cloud.
222  if (not pointCloud.header.frame_id.empty())
223  {
224  return pointCloud.header.frame_id;
225  }
226 
227  // check if fallback solution is available
229  << deactivateSpam(100)
230  << "The point cloud header does not provide a frame_id. Using property instead";
231 
232  ARMARX_CHECK_NOT_EMPTY(params.pointCloudNodeName)
233  << "The `pointCloudNodeName` property must be provided as the point cloud "
234  "header "
235  "frame id is not set!";
236 
237  return params.pointCloudNodeName;
238  }();
239 
240  if (not robot)
241  {
243  << "The robot is not available. Won't transform point cloud";
244  return std::nullopt;
245  }
246 
247  Eigen::Matrix4f globalSensorPose = Eigen::Matrix4f::Identity();
248  if (pointCloudNodeName != armarx::GlobalFrame)
249  {
250  ARMARX_CHECK_NOT_NULL(robot);
251  const auto node = robot->getRobotNode(pointCloudNodeName);
252  ARMARX_CHECK_NOT_NULL(node) << "No robot node with name `" << pointCloudNodeName << "`";
253 
254  globalSensorPose = node->getGlobalPose();
255 
256  pcl::transformPointCloud(pointCloud, pointCloud, globalSensorPose);
257  }
258 
259  pointCloud.header.frame_id = armarx::GlobalFrame;
260 
261  return OriginInfo{.name = pointCloudNodeName, .pose = globalSensorPose};
262  }
263 
264  void
266  {
267  using namespace armarx::RemoteGui::Client;
268 
269  GridLayout grid;
270  int row = 0;
271 
272  tab.checkFinite.setName("Check Finite");
273  tab.checkFinite.setValue(params.checkFinite);
274 
275  tab.pointSizeInPixels.setName("Point Size [pixels]");
276  tab.pointSizeInPixels.setDecimals(2);
277  tab.pointSizeInPixels.setRange(0, 100);
278  tab.pointSizeInPixels.setSteps(100);
279  tab.pointSizeInPixels.setValue(params.pointSizeInPixels);
280 
281  tab.labeled.setName("Color by Labels");
282  tab.labeled.setValue(params.labeled);
283 
284  grid.add(Label(tab.checkFinite.getName()), {row, 0}).add(tab.checkFinite, {row, 1});
285  row++;
286  grid.add(Label(tab.pointSizeInPixels.getName()), {row, 0})
287  .add(tab.pointSizeInPixels, {row, 1});
288  row++;
289  grid.add(Label(tab.labeled.getName()), {row, 0}).add(tab.labeled, {row, 1});
290  row++;
291 
292  VBoxLayout root = {grid, VSpacer()};
293  RemoteGui_createTab(getName(), root, &tab);
294  }
295 
296  void
298  {
299  std::scoped_lock lock(paramMutex);
300  if (tab.checkFinite.hasValueChanged())
301  {
302  params.checkFinite = tab.checkFinite.getValue();
303  }
304  if (tab.pointSizeInPixels.hasValueChanged())
305  {
306  params.pointSizeInPixels = tab.pointSizeInPixels.getValue();
307  }
308  if (tab.labeled.hasValueChanged())
309  {
310  params.labeled = tab.labeled.getValue();
311  }
312  }
313 
314 
315 } // namespace visionx
armarx::armem::client::plugins::ReaderWriterPlugin::isAvailable
bool isAvailable() const
Definition: ReaderWriterPlugin.h:80
PointCloudToArViz.h
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:80
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::armem::robot_state::VirtualRobotReader::getRobot
VirtualRobot::RobotPtr getRobot(const std::string &name, const armem::Time &timestamp=armem::Time::Invalid(), const VirtualRobot::RobotIO::RobotDescription &loadMode=VirtualRobot::RobotIO::RobotDescription::eStructure)
Definition: VirtualRobotReader.cpp:57
visionx::PointCloudToArViz::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: PointCloudToArViz.cpp:48
visionx::PointCloudToArViz::setPointSizeInPixels
virtual void setPointSizeInPixels(float pointSizeInPixels, const Ice::Current &=Ice::emptyCurrent) override
Definition: PointCloudToArViz.cpp:183
visionx::PointCloudProcessor::getPointCloudFormat
MetaPointCloudFormatPtr getPointCloudFormat(std::string providerName)
Definition: PointCloudProcessor.cpp:506
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
visionx::PointCloudToArViz::process
void process() override
Definition: PointCloudToArViz.cpp:118
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
visionx::PointCloudToArVizPropertyDefinitions
Property definitions of PointCloudToArViz.
Definition: PointCloudToArViz.h:57
armarx::RemoteGui::Client::VBoxLayout
Definition: Widgets.h:167
Pose.h
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::RemoteGui::Client::GridLayout::add
GridLayout & add(Widget const &child, Pos pos, Span span=Span{1, 1})
Definition: Widgets.cpp:412
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
armarx::RemoteGui::Client::Widget::setName
void setName(std::string const &name)
Definition: Widgets.cpp:19
armarx::armem::robot_state::VirtualRobotReader::synchronizeRobot
bool synchronizeRobot(VirtualRobot::Robot &robot, const armem::Time &timestamp) const
Definition: VirtualRobotReader.cpp:20
visionx::tools::isLabeled
bool isLabeled(PointContentType type)
Definition: PointCloudConversions.cpp:108
ARMARX_CHECK_NOT_EMPTY
#define ARMARX_CHECK_NOT_EMPTY(c)
Definition: ExpressionException.h:224
armarx::ManagedIceObject::addPlugin
PluginT * addPlugin(const std::string prefix="", ParamsT &&...params)
Definition: ManagedIceObject.h:182
visionx::PointCloudToArVizPropertyDefinitions::PointCloudToArVizPropertyDefinitions
PointCloudToArVizPropertyDefinitions(std::string prefix)
Definition: PointCloudToArViz.cpp:40
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:29
armarx::RemoteGui::Client::VSpacer
Definition: Widgets.h:204
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
Clock.h
visionx::voxelgrid::Label
uint32_t Label
Type of an object label.
Definition: types.h:7
armarx::RemoteGui::Client::GridLayout
Definition: Widgets.h:186
visionx::PointCloudToArViz::PointCloudToArViz
PointCloudToArViz()
Definition: PointCloudToArViz.cpp:76
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
FramedPose.h
armarx::viz::PointCloud::pointSizeInPixels
PointCloud & pointSizeInPixels(float s)
Definition: PointCloud.h:55
visionx::PointCloudToArViz::RemoteGui_update
void RemoteGui_update() override
Definition: PointCloudToArViz.cpp:297
visionx::PointCloudProcessor::waitForPointClouds
bool waitForPointClouds(int milliseconds=1000)
Wait for new PointClouds.
Definition: PointCloudProcessor.cpp:433
armarx::viz::PointCloud
Definition: PointCloud.h:21
VirtualRobotReader.h
visionx::PointCloudToArViz::getDefaultName
std::string getDefaultName() const override
Definition: PointCloudToArViz.cpp:82
visionx::PointCloudToArViz::onDisconnectPointCloudProcessor
void onDisconnectPointCloudProcessor() override
Definition: PointCloudToArViz.cpp:108
visionx::PointCloudProcessorPropertyDefinitions
Properties of PointCloudProcessor.
Definition: PointCloudProcessor.h:173
visionx::PointCloudProcessor::getPointClouds
int getPointClouds(const PointCloudPtrT &pointCloudPtr)
Poll PointClouds from provider.
Definition: PointCloudProcessor.h:373
visionx::PointCloudToArViz::onInitPointCloudProcessor
void onInitPointCloudProcessor() override
Definition: PointCloudToArViz.cpp:88
armarx::armem::client::plugins::ReaderWriterPlugin::get
T & get()
Definition: ReaderWriterPlugin.h:87
armarx::LightweightRemoteGuiComponentPluginUser::RemoteGui_startRunningTask
void RemoteGui_startRunningTask()
Definition: LightweightRemoteGuiComponentPlugin.cpp:110
ExpressionException.h
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::ArVizComponentPluginUser::arviz
armarx::viz::Client arviz
Definition: ArVizComponentPlugin.h:43
visionx::PointCloudToArViz::createRemoteGuiTab
void createRemoteGuiTab()
Definition: PointCloudToArViz.cpp:265
armarx::LightweightRemoteGuiComponentPluginUser::RemoteGui_createTab
void RemoteGui_createTab(std::string const &name, RemoteGui::Client::Widget const &rootWidget, RemoteGui::Client::Tab *tab)
Definition: LightweightRemoteGuiComponentPlugin.cpp:95
IceUtil::Handle< class PropertyDefinitionContainer >
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::PointCloudToArViz::onConnectPointCloudProcessor
void onConnectPointCloudProcessor() override
Definition: PointCloudToArViz.cpp:95
Logging.h
armarx::viz::Client::layer
Layer layer(std::string const &name) const
Definition: Client.cpp:73
visionx::PointCloudToArViz::onExitPointCloudProcessor
void onExitPointCloudProcessor() override
Definition: PointCloudToArViz.cpp:113
armarx::core::time::Duration::MicroSeconds
static Duration MicroSeconds(std::int64_t microSeconds)
Constructs a duration in microseconds.
Definition: Duration.cpp:27
armarx::RemoteGui::Client
Definition: EigenWidgets.cpp:8
armarx::viz::PointCloud::pointCloud
PointCloud & pointCloud(const PointCloudT &cloud)
Draw a point cloud.
Definition: PointCloud.h:194
armarx::viz::Layer
Definition: Layer.h:12
armarx::viz::PointCloud::checkFinite
PointCloud & checkFinite(bool enabled=true)
Enable or disable checking whether points are finite when adding them (disabled by default).
Definition: PointCloud.h:41
visionx::PointCloudProcessor::getPointCloudProviderNames
std::vector< std::string > getPointCloudProviderNames() const
Get the names of providers for which usingPointCloudProvider() has been called.
Definition: PointCloudProcessor.cpp:381
visionx::PointCloudToArViz::setCustomTransform
virtual void setCustomTransform(const armarx::PoseBasePtr &transform, const Ice::Current &=Ice::emptyCurrent) override
Definition: PointCloudToArViz.cpp:190