diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index 4722e9c5ce8deaabfc33a917c9b0c744d1a781fd..12dc68399fc92b3d32816dd212d75fc4b0cdab8c 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -1,23 +1,24 @@
-#include <regex>
-#include <fstream>
-
#include "VisualizationRobot.h"
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <fstream>
+#include <regex>
+
#include <VirtualRobot/SceneObject.h>
-#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
namespace armarx::viz::coin
{
namespace
{
- VirtualRobot::RobotPtr loadRobot(std::string const& project, std::string const& filename)
+ VirtualRobot::RobotPtr
+ loadRobot(std::string const& project, std::string const& filename)
{
- VirtualRobot::RobotPtr result;
+ VirtualRobot::RobotPtr result;
if (filename.empty())
{
@@ -32,7 +33,8 @@ namespace armarx::viz::coin
ARMARX_INFO << deactivateSpam()
<< "You specified the absolute path to the robot file:"
<< "\n\t'" << filename << "'"
- << "\nConsider specifying the containing ArmarX package and relative data path instead to "
+ << "\nConsider specifying the containing ArmarX package and relative "
+ "data path instead to "
<< "improve portability to other systems.";
}
// We need to always check that the file is readable otherwise, VirtualRobot::RobotIO::loadRobot crashes
@@ -59,9 +61,7 @@ namespace armarx::viz::coin
if (result)
{
result->setThreadsafe(false);
- // Do we want to propagate joint values? Probably not...
- // Closing the hand on the real robot could be implemented on another level
- result->setPropagatingJointValuesEnabled(false);
+ result->setPropagatingJointValuesEnabled(true);
}
else
{
@@ -88,7 +88,8 @@ namespace armarx::viz::coin
static std::vector<RobotInstancePool> robotCache;
- LoadedRobot getRobotFromCache(std::string const& project, std::string const& filename)
+ LoadedRobot
+ getRobotFromCache(std::string const& project, std::string const& filename)
{
// We can use a global variable, since this code is only executed in the GUI thread
@@ -104,7 +105,8 @@ namespace armarx::viz::coin
if (instancePool.usedInstances < instancePool.robots.size())
{
// 1) We have still unused instances in the pool ==> Just return one
- ARMARX_DEBUG << "Reusing robot instance from cache " << VAROUT(project) << ", " << VAROUT(filename);
+ ARMARX_DEBUG << "Reusing robot instance from cache " << VAROUT(project)
+ << ", " << VAROUT(filename);
result.robot = instancePool.robots[instancePool.usedInstances];
instancePool.usedInstances += 1;
}
@@ -112,14 +114,17 @@ namespace armarx::viz::coin
else
{
// 2) We do not have unused instances in the pool ==> Clone one
- ARMARX_DEBUG << "Cloning robot from cache " << VAROUT(project) << ", " << VAROUT(filename);
+ ARMARX_DEBUG << "Cloning robot from cache " << VAROUT(project) << ", "
+ << VAROUT(filename);
if (instancePool.robots.size() > 0)
{
- VirtualRobot::RobotPtr const& robotToClone = instancePool.robots.front();
+ VirtualRobot::RobotPtr const& robotToClone =
+ instancePool.robots.front();
float scaling = 1.0f;
bool preventCloningMeshesIfScalingIs1 = true;
- result.robot = robotToClone->clone(nullptr, scaling, preventCloningMeshesIfScalingIs1);
+ result.robot = robotToClone->clone(
+ nullptr, scaling, preventCloningMeshesIfScalingIs1);
// Insert the cloned robot into the instance pool
instancePool.robots.push_back(result.robot);
@@ -127,8 +132,10 @@ namespace armarx::viz::coin
}
else
{
- ARMARX_WARNING << "Encountered empty robot instance pool while trying to clone new instance"
- << "\nRobot: " << VAROUT(project) << ", " << VAROUT(filename)
+ ARMARX_WARNING << "Encountered empty robot instance pool while trying "
+ "to clone new instance"
+ << "\nRobot: " << VAROUT(project) << ", "
+ << VAROUT(filename)
<< "\nUsed instances: " << instancePool.usedInstances
<< "\nRobots: " << instancePool.robots.size();
}
@@ -137,7 +144,8 @@ namespace armarx::viz::coin
}
}
- ARMARX_DEBUG << "Loading robot from file " << VAROUT(project) << ", " << VAROUT(filename);
+ ARMARX_DEBUG << "Loading robot from file " << VAROUT(project) << ", "
+ << VAROUT(filename);
result.robot = loadRobot(project, filename);
if (result.robot)
{
@@ -146,13 +154,15 @@ namespace armarx::viz::coin
instancePool.filename = filename;
instancePool.robots.push_back(result.robot);
instancePool.usedInstances = 1;
- } else
+ }
+ else
{
- ARMARX_WARNING << deactivateSpam(5) << "Robot " << VAROUT(project) << ", " << VAROUT(filename) << "could not be loaded!";
+ ARMARX_WARNING << deactivateSpam(5) << "Robot " << VAROUT(project) << ", "
+ << VAROUT(filename) << "could not be loaded!";
}
return result;
}
- }
+ } // namespace
VisualizationRobot::~VisualizationRobot()
{
@@ -160,7 +170,8 @@ namespace armarx::viz::coin
{
if (instancePool.project == loaded.project && instancePool.filename == loaded.filename)
{
- ARMARX_DEBUG << "Removing robot from chace " << VAROUT(loaded.project) << ", " << VAROUT(loaded.filename);
+ ARMARX_DEBUG << "Removing robot from chace " << VAROUT(loaded.project) << ", "
+ << VAROUT(loaded.filename);
std::vector<VirtualRobot::RobotPtr>& robots = instancePool.robots;
auto robotIter = std::find(robots.begin(), robots.end(), loaded.robot);
if (robotIter != robots.end())
@@ -176,7 +187,8 @@ namespace armarx::viz::coin
{
ARMARX_WARNING << "Expected there to be at least one used instance "
<< "while trying to put robot instance back into the pool"
- << "\nRobot: " << VAROUT(loaded.project) << ", " << VAROUT(loaded.filename)
+ << "\nRobot: " << VAROUT(loaded.project) << ", "
+ << VAROUT(loaded.filename)
<< "\nUsed instances: " << instancePool.usedInstances;
}
}
@@ -184,12 +196,14 @@ namespace armarx::viz::coin
}
}
- bool VisualizationRobot::update(ElementType const& element)
+ bool
+ VisualizationRobot::update(ElementType const& element)
{
IceUtil::Time time_start = IceUtil::Time::now();
- (void) time_start;
+ (void)time_start;
- bool robotChanged = loaded.project != element.project || loaded.filename != element.filename;
+ bool robotChanged =
+ loaded.project != element.project || loaded.filename != element.filename;
if (robotChanged)
{
// The robot file changed, so reload the robot
@@ -199,8 +213,7 @@ namespace armarx::viz::coin
{
ARMARX_WARNING << deactivateSpam(10)
<< "Robot will not visualized since it could not be loaded."
- << "\nID: " << element.id
- << "\nProject: " << element.project
+ << "\nID: " << element.id << "\nProject: " << element.project
<< "\nFilename: " << element.filename;
return true;
}
@@ -253,10 +266,8 @@ namespace armarx::viz::coin
if (loadedDrawStyle & data::ModelDrawStyle::OVERRIDE_COLOR)
{
- if (loadedColor.r != element.color.r
- || loadedColor.g != element.color.g
- || loadedColor.b != element.color.b
- || loadedColor.a != element.color.a)
+ if (loadedColor.r != element.color.r || loadedColor.g != element.color.g ||
+ loadedColor.b != element.color.b || loadedColor.a != element.color.a)
{
int numChildren = node->getNumChildren();
for (int i = 0; i < numChildren; i++)
@@ -294,7 +305,8 @@ namespace armarx::viz::coin
return true;
}
- void VisualizationRobot::recreateVisualizationNodes(int drawStyle)
+ void
+ VisualizationRobot::recreateVisualizationNodes(int drawStyle)
{
VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
if (drawStyle & data::ModelDrawStyle::COLLISION)
@@ -331,8 +343,9 @@ namespace armarx::viz::coin
}
}
- void clearRobotCache()
+ void
+ clearRobotCache()
{
robotCache.clear();
}
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt
index e3467eb9a250af792c5c53d578b0555b49a95efa..ce15a37df98b729bd0a0dc875ccbac13aae2108f 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt
@@ -8,12 +8,17 @@ armarx_add_library(
SimoxUtility
RobotAPI::Core
RobotAPI::Aron::Common
+
+ ArViz
+ RobotAPIArmarXObjects
SOURCES
datatypes/CommonPlace.cpp
CommonPlaceLoader.cpp
+ Visu.cpp
HEADERS
datatypes/CommonPlace.h
CommonPlaceLoader.h
+ Visu.h
)
add_library(${PROJECT_NAME}::PriorKnowledge::util::CommonPlaceLoader ALIAS ${PROJECT_NAME}PriorKnowledgeCommonPlaceLoaderUtil)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cfd68864ff87c6490af33be5d40645051b0e4fe7
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.cpp
@@ -0,0 +1,24 @@
+#include "Visu.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+
+namespace armarx::priorknowledge::util::common_place
+{
+ viz::Layer
+ Visu::commonPlaceToLayer(const std::string& layerName,
+ const std::map<std::string, CommonPlaceData>& commonPlaceData) const
+ {
+ auto layer = arviz.layer(layerName);
+ for (auto& [id, data] : commonPlaceData)
+ {
+ auto o = armarx::viz::Object("");
+ o = o.fileByObjectFinder(armarx::ObjectID(data.objectId));
+ o.pose(data.globalPose).alpha(0.5);
+ layer.add(o);
+ }
+ return layer;
+ }
+
+} // namespace armarx::priorknowledge::util::common_place
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.h b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..e4df667ca056b92db65053834a1fea75b75d8eb9
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
+
+#include "datatypes/CommonPlace.h"
+
+namespace armarx::priorknowledge::util::common_place
+{
+
+ class Visu
+ {
+ public:
+ struct CommonPlaceData
+ {
+ Eigen::Matrix4f globalPose;
+ std::string objectId;
+ };
+
+ Visu(viz::Client& arviz) : arviz(arviz)
+
+ {
+ }
+
+ ~Visu() = default;
+
+ viz::Layer commonPlaceToLayer(const std::string& layerName,
+ const std::map<std::string, CommonPlaceData>& data) const;
+
+
+ public:
+ struct Settings
+ {
+
+ } settings;
+
+ protected:
+ viz::Client& arviz;
+ };
+} // namespace armarx::priorknowledge::util::common_place