Skip to content
Snippets Groups Projects
Commit 914f4ce2 authored by Rainer Kartmann's avatar Rainer Kartmann
Browse files

Add LayerBlackWhitelist to DebugDrawerToArViz

parent e324151a
No related branches found
No related tags found
1 merge request!69Add DebugDrawerToArViz
Hide whitespace changes
Inline Side-by-side
Showing
with 241 additions and 21 deletions
source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
+ 162
− 14
View file @ 914f4ce2
......@@ -27,6 +27,9 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "BlackWhitelistUpdate.h"
#define FUNCTION_NOT_IMPLEMENTED_MESSAGE \
"Function DebugDrawerToArViz::" << __FUNCTION__ << "(): Not implemented."
......@@ -81,6 +84,29 @@ namespace armarx
}
void DebugDrawerToArViz::updateBlackWhiteList(const BlackWhitelistUpdate& update, const Ice::Current&)
{
std::scoped_lock lock(mutex);
armarx::updateBlackWhitelist(layerBlackWhitelist, update);
ARMARX_VERBOSE << "Updated layer black-whitelist: \n" << layerBlackWhitelist;
// Remove all excluded layers.
std::vector<viz::Layer> cleared;
for (const auto& [name, layer] : layers)
{
if (layerBlackWhitelist.isExcluded(name))
{
cleared.push_back(arviz.layer(name));
}
}
if (!cleared.empty())
{
arviz.commit(cleared);
}
}
void DebugDrawerToArViz::exportScene(const std::string&, const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
......@@ -94,18 +120,30 @@ namespace armarx
void DebugDrawerToArViz::setPoseVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
setAndCommit(layer, viz::Pose(name).pose(toEigen(globalPose)));
}
void DebugDrawerToArViz::setScaledPoseVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, Ice::Float scale, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
setAndCommit(layer, viz::Pose(name).pose(toEigen(globalPose)).scale(scale));
}
void DebugDrawerToArViz::setLineVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, Ice::Float lineWidth, const DrawColor& color, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
setAndCommit(layer, viz::Polygon(name).addPoint(toEigen(globalPosition1)).addPoint(toEigen(globalPosition2))
.lineColor(toViz(color)).lineWidth(lineWidth).color(simox::Color::black(0)));
}
......@@ -113,6 +151,10 @@ namespace armarx
void DebugDrawerToArViz::setLineSetVisu(const std::string& layerName, const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layerName))
{
return;
}
viz::Layer& layer = getLayer(layerName);
ARMARX_CHECK_EQUAL(lineSet.points.size() % 2, 0) << VAROUT(lineSet.points.size());
......@@ -144,12 +186,20 @@ namespace armarx
void DebugDrawerToArViz::setBoxVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
setAndCommit(layer, viz::Box(name).pose(toEigen(globalPose)).size(toEigen(dimensions)).color(toViz(color)));
}
void DebugDrawerToArViz::setTextVisu(const std::string& layer, const std::string& name, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Int size, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
setAndCommit(layer, viz::Text(name).text(text).position(toEigen(globalPosition))
.scale(size).color(toViz(color)));
}
......@@ -157,12 +207,20 @@ namespace armarx
void DebugDrawerToArViz::setSphereVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Float radius, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
setAndCommit(layer, viz::Sphere(name).position(toEigen(globalPosition)).radius(radius).color(toViz(color)));
}
void DebugDrawerToArViz::setPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerPointCloud& pointCloud, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
viz::PointCloud cloud(name);
for (const auto& p : pointCloud.points)
......@@ -175,6 +233,10 @@ namespace armarx
void DebugDrawerToArViz::setColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
viz::PointCloud cloud(name);
for (const auto& p : pointCloud.points)
......@@ -192,6 +254,10 @@ namespace armarx
void DebugDrawerToArViz::set24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
viz::PointCloud cloud(name);
for (const auto& p : pointCloud.points)
......@@ -209,6 +275,10 @@ namespace armarx
void DebugDrawerToArViz::setPolygonVisu(const std::string& layer, const std::string& name, const PolygonPointList& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, Ice::Float lineWidth, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
viz::Polygon poly(name);
for (const auto& p : polygonPoints)
......@@ -221,6 +291,10 @@ namespace armarx
void DebugDrawerToArViz::setTriMeshVisu(const std::string& layer, const std::string& name, const DebugDrawerTriMesh& triMesh, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
std::vector<Eigen::Vector3f> vertices;
std::vector<viz::data::Color> colors;
......@@ -249,6 +323,10 @@ namespace armarx
void DebugDrawerToArViz::setArrowVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, Ice::Float length, Ice::Float width, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
setAndCommit(layer, viz::Arrow(name).position(toEigen(position)).direction(toEigen(direction))
.color(toViz(color)).width(width).length(length));
}
......@@ -256,6 +334,10 @@ namespace armarx
void DebugDrawerToArViz::setCylinderVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, Ice::Float length, Ice::Float radius, const DrawColor& color, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
setAndCommit(layer, viz::Cylinder(name)
.fromTo(toEigen(globalPosition), toEigen(globalPosition) + length * toEigen(direction))
.color(toViz(color)).radius(radius));
......@@ -264,6 +346,10 @@ namespace armarx
void DebugDrawerToArViz::setCircleArrowVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
setAndCommit(layer, viz::ArrowCircle(name).position(toEigen(globalPosition)).normal(toEigen(directionVec))
.radius(radius).completion(circleCompletion).color(toViz(color)).width(width));
}
......@@ -272,6 +358,10 @@ namespace armarx
void DebugDrawerToArViz::setRobotVisu(const std::string& layer, const std::string& name, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyleType, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
viz::Robot robot = viz::Robot(name).file(armarxProject, robotFile);
switch (drawStyleType)
......@@ -291,6 +381,10 @@ namespace armarx
void DebugDrawerToArViz::updateRobotPose(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
if (auto it = robots.find(std::make_pair(layer, name)); it != robots.end())
{
viz::Robot& robot = it->second;
......@@ -302,6 +396,10 @@ namespace armarx
void DebugDrawerToArViz::updateRobotConfig(const std::string& layer, const std::string& name, const NameValueMap& configuration, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
if (auto it = robots.find(std::make_pair(layer, name)); it != robots.end())
{
viz::Robot& robot = it->second;
......@@ -313,6 +411,10 @@ namespace armarx
void DebugDrawerToArViz::updateRobotColor(const std::string& layer, const std::string& name, const DrawColor& color, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
if (auto it = robots.find(std::make_pair(layer, name)); it != robots.end())
{
viz::Robot& robot = it->second;
......@@ -330,6 +432,10 @@ namespace armarx
void DebugDrawerToArViz::removeRobotVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
robots.erase(std::make_pair(layer, name));
removeAndCommit(layer, name);
}
......@@ -397,72 +503,114 @@ namespace armarx
void DebugDrawerToArViz::removePoseVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removeLineVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removeLineSetVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removeBoxVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removeTextVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removeSphereVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removePointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removeColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::remove24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removePolygonVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removeTriMeshVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removeArrowVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removeCylinderVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
void DebugDrawerToArViz::removeCircleVisu(const std::string& layer, const std::string& name, const Ice::Current&)
{
std::scoped_lock lock(mutex);
removeAndCommit(layer, name);
if (layerBlackWhitelist.isIncluded(layer))
{
removeAndCommit(layer, name);
}
}
......
source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h
+ 13
− 2
View file @ 914f4ce2
......@@ -25,9 +25,11 @@
#include <map>
#include <mutex>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/interface/visualization/DebugDrawerToArViz.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
#include "BlackWhitelist.h"
namespace armarx
{
......@@ -35,13 +37,17 @@ namespace armarx
/**
* @brief Passes updates from DebugDrawerInterface to ArViz.
*/
class DebugDrawerToArViz : virtual public armarx::DebugDrawerInterface
class DebugDrawerToArViz : virtual public armarx::DebugDrawerToArvizInterface
{
public:
void setArViz(viz::Client arviz);
// BlackWhitelistTopic interface
public:
void updateBlackWhiteList(const BlackWhitelistUpdate& update, const Ice::Current& = Ice::emptyCurrent) override;
// DebugDrawerInterface interface
public:
......@@ -188,6 +194,11 @@ namespace armarx
void removeAndCommit(const std::string& layerName, const std::string& name);
public:
armarx::StringBlackWhitelist layerBlackWhitelist;
private:
const std::string DEBUG_LAYER_NAME = "debug";
......
source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.cpp
+ 23
− 2
View file @ 914f4ce2
......@@ -22,6 +22,8 @@
#include "DebugDrawerToArVizComponent.h"
#include "BlackWhitelistUpdate.h"
namespace armarx
{
......@@ -30,6 +32,17 @@ namespace armarx
{
defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates",
"Name of the topic the DebugDrawer listens to.");
defineOptionalProperty<std::string>(
"LayerBlackWhitelistTopic", "DebugDrawerToArVizLayerBlackWhitelistUpdates",
"The layer where updates to the layer black-whitelist are published.");
defineOptionalProperty<std::vector<std::string>>("LayerWhitelist", {},
"If not empty, layers are shown (comma separated list).")
.map("[empty whitelist]", {});
defineOptionalProperty<std::vector<std::string>>("LayerBlacklist", {},
"These layers will never be shown (comma separated list).")
.map("[empty blacklist]", {});
}
......@@ -41,7 +54,16 @@ namespace armarx
void DebugDrawerToArVizComponent::onInitComponent()
{
{
BlackWhitelistUpdate update;
getProperty(update.whitelist.set, "LayerWhitelist");
getProperty(update.blacklist.set, "LayerBlacklist");
armarx::updateBlackWhitelist(DebugDrawerToArViz::layerBlackWhitelist, update);
ARMARX_VERBOSE << "Layer black-white-list: \n" << DebugDrawerToArViz::layerBlackWhitelist;
}
usingTopicFromProperty("DebugDrawerTopicName");
usingTopicFromProperty("LayerBlackWhitelistTopic");
}
......@@ -63,7 +85,6 @@ namespace armarx
armarx::PropertyDefinitionsPtr DebugDrawerToArVizComponent::createPropertyDefinitions()
{
return armarx::PropertyDefinitionsPtr(new DebugDrawerToArVizPropertyDefinitions(
getConfigIdentifier()));
return armarx::PropertyDefinitionsPtr(new DebugDrawerToArVizPropertyDefinitions(getConfigIdentifier()));
}
}
source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h
+ 3
− 3
View file @ 914f4ce2
......@@ -25,7 +25,7 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/interface/visualization/DebugDrawerToArViz.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
......@@ -59,8 +59,8 @@ namespace armarx
*/
class DebugDrawerToArVizComponent :
virtual public armarx::Component,
virtual public armarx::ArVizComponentPluginUser,
virtual public armarx::DebugDrawerToArViz // Implements armarx::DebugDrawerInterface
virtual public armarx::DebugDrawerToArViz, // Implements armarx::DebugDrawerToArvizInterface
virtual public armarx::ArVizComponentPluginUser
{
public:
......
source/RobotAPI/interface/CMakeLists.txt
+ 1
− 0
View file @ 914f4ce2
......@@ -82,6 +82,7 @@ set(SLICE_FILES
components/FrameTrackingInterface.ice
visualization/DebugDrawerInterface.ice
visualization/DebugDrawerToArViz.ice
ArViz.ice
ArViz/Elements.ice
......
source/RobotAPI/interface/visualization/DebugDrawerToArViz.ice 0 → 100644
+ 39
− 0
View file @ 914f4ce2
/*
* This file is part of ArmarX.
*
* Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package ArmarX::RobotAPI
* @author Nikolaus Vahrenkamp
* @copyright 2014
* @license http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#pragma once
#include <RobotAPI/interface/core/BlackWhitelist.ice>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.ice>
module armarx
{
interface DebugDrawerToArvizInterface extends DebugDrawerInterface, BlackWhitelistTopic
{
};
};
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment