From 3538226ff01f346f516bd09f33b10ab9deb6fc0a Mon Sep 17 00:00:00 2001
From: Nikolaus Vahrenkamp <vahrenkamp@kit.edu>
Date: Fri, 6 Mar 2015 17:30:12 +0100
Subject: [PATCH] Added robot visualization to debug drawer
---
.../DebugDrawer/DebugDrawerComponent.cpp | 229 ++++++++++++++++++
.../DebugDrawer/DebugDrawerComponent.h | 43 +++-
.../visualization/DebugDrawerInterface.ice | 20 ++
3 files changed, 291 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index f0df39894..b08c2f3fd 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -25,6 +25,7 @@
#include "DebugDrawerComponent.h"
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <Inventor/nodes/SoUnits.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoMaterial.h>
@@ -36,6 +37,8 @@
#include <Inventor/nodes/SoComplexity.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoPointSet.h>
+#include <Core/core/system/ArmarXDataPath.h>
+#include <Core/core/system/cmake/CMakePackageFinder.h>
using namespace VirtualRobot;
@@ -400,7 +403,126 @@ void DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData &d)
layer.addedArrowVisualizations[d.name] = sep;
layer.mainNode->addChild(sep);
+}
+
+void DebugDrawerComponent::drawRobot(const DebugDrawerComponent::RobotData &d)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+ auto& layer=requestLayer(d.layerName);
+
+ if (!d.active)
+ {
+ ARMARX_INFO << "Removing robot " << d.name;
+ removeRobot(d.layerName,d.name);
+ return;
+ }
+ // load or get robot
+ RobotPtr rob = requestRobot(d);
+ if (!rob)
+ {
+ ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name << " at layer " << d.layerName;
+ return;
+ }
+
+ if (d.updatePose)
+ {
+ rob->setGlobalPose(d.globalPose);
+ }
+ if (d.updateConfig)
+ {
+ rob->setJointValues(d.configuration);
+ }
+ if (d.updateColor)
+ {
+ SoSeparator *sep = layer.addedRobotVisualizations[d.name];
+ if (!sep || sep->getNumChildren()<2)
+ {
+ ARMARX_ERROR << "Internal robot layer error1";
+ return;
+ }
+
+ SoMaterial* m = dynamic_cast<SoMaterial*>(sep->getChild(0));
+ if (!m)
+ {
+ ARMARX_ERROR << "Internal robot layer error2";
+ return;
+ }
+ if (d.color.isNone())
+ {
+ m->setOverride(false);
+ } else
+ {
+ m->diffuseColor = SbColor(d.color.r,d.color.g,d.color.b);
+ m->ambientColor = SbColor(0,0,0);
+ m->transparency = std::max(0.0f,d.color.transparency);
+ m->setOverride(true);
+ }
+ }
+}
+
+VirtualRobot::RobotPtr DebugDrawerComponent::requestRobot(const DebugDrawerComponent::RobotData &d)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+
+ auto& layer=requestLayer(d.layerName);
+ std::string entryName = "__" + d.layerName + "__" + d.name + "__";
+
+ if (activeRobots.find(entryName) != activeRobots.end())
+ return activeRobots[entryName];
+
+ //ARMARX_INFO << "Loading robot " << entryName;
+ VirtualRobot::RobotPtr result;
+
+ if (d.robotFile.empty())
+ {
+ ARMARX_INFO << deactivateSpam() << "No robot defined for layer " << d.layerName << ", with name " << d.name;
+ return result;
+ }
+
+ if (!d.armarxProject.empty())
+ {
+ ARMARX_INFO << "Adding to datapaths of " << d.armarxProject;
+ armarx::CMakePackageFinder finder(d.armarxProject);
+ if (!finder.packageFound())
+ {
+ ARMARX_WARNING << "ArmarX Package " << d.armarxProject << " has not been found!";
+ } else
+ {
+ ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
+ armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
+ }
+ }
+
+ // load robot
+ std::string filename = d.robotFile;
+ ArmarXDataPath::getAbsolutePath(filename,filename);
+ ARMARX_INFO << "Loading robot from " << filename;
+ try
+ {
+ result = RobotIO::loadRobot(filename);
+ } catch (...)
+ {
+ ARMARX_IMPORTANT << "Robot loading failed, file:" << filename;
+ return result;
+ }
+ ARMARX_INFO << "Loaded robot";
+
+ SoSeparator *sep = new SoSeparator;
+ layer.mainNode->addChild(sep);
+
+ SoMaterial *m = new SoMaterial;
+ m->setOverride(false);
+ sep->addChild(m);
+
+ boost::shared_ptr<CoinVisualization> robVisu = result->getVisualization<CoinVisualization>(VirtualRobot::SceneObject::Full);
+ SoNode *sepRob = robVisu->getCoinVisualization();
+ sep->addChild(sepRob);
+
+ activeRobots[entryName] = result;
+ layer.addedRobotVisualizations[d.name] = sep;
+
+ return result;
}
void DebugDrawerComponent::removeLine(const std::string& layerName, const std::string &name)
@@ -522,6 +644,30 @@ void DebugDrawerComponent::removeArrow(const std::string &layerName, const std::
layer.addedArrowVisualizations.erase(name);
}
+void DebugDrawerComponent::removeRobot(const std::string &layerName, const std::string &name)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+
+ // process active robots
+ std::string entryName = "__" + layerName + "__" + name + "__";
+ if (activeRobots.find(entryName) != activeRobots.end())
+ activeRobots.erase(entryName);
+
+ // process visualizations
+ if(!hasLayer(layerName))
+ {
+ return;
+ }
+ auto& layer=layers.at(layerName);
+ if(layer.addedRobotVisualizations.find(name) == layer.addedRobotVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.addedRobotVisualizations[name]);
+ layer.addedRobotVisualizations.erase(name);
+}
+
void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string &name)
{
ScopedRecursiveLockPtr l = getScopedLock();
@@ -923,6 +1069,83 @@ void DebugDrawerComponent::removeArrowDebugLayerVisu(const std::string &arrowNam
removeArrowVisu(DEBUG_LAYER_NAME, arrowName);
}
+void DebugDrawerComponent::setRobotVisu(const std::string &layerName, const std::string &robotName, const std::string &robotFile, const std::string &armarxProject, const Ice::Current &)
+{
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + robotName + "__";
+ RobotData &d = accumulatedUpdateData.robots[entryName];
+
+ d.robotFile = robotFile;
+ d.armarxProject = armarxProject;
+
+ d.layerName = layerName;
+ d.name = robotName;
+ d.active = true;
+}
+
+void DebugDrawerComponent::updateRobotPose(const std::string &layerName, const std::string &robotName, const PoseBasePtr &globalPose, const Ice::Current &)
+{
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + robotName + "__";
+ RobotData &d = accumulatedUpdateData.robots[entryName];
+
+ // update data
+ d.update = true;
+ d.updatePose = true;
+ d.globalPose = PosePtr::dynamicCast(globalPose)->toEigen();
+
+ d.layerName = layerName;
+ d.name = robotName;
+ d.active = true;
+}
+
+void DebugDrawerComponent::updateRobotConfig(const std::string &layerName, const std::string &robotName, const std::map<std::string, float> &configuration, const Ice::Current &)
+{
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + robotName + "__";
+ RobotData &d = accumulatedUpdateData.robots[entryName];
+
+ // update data
+ d.update = true;
+ d.updateConfig = true;
+ d.layerName = layerName;
+ d.name = robotName;
+ for(auto& it : configuration)
+ {
+ d.configuration[it.first] = it.second;
+ }
+ d.active = true;
+}
+
+void DebugDrawerComponent::updateRobotColor(const std::string &layerName, const std::string &robotName, const DrawColor &c, const Ice::Current &)
+{
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + robotName + "__";
+ RobotData &d = accumulatedUpdateData.robots[entryName];
+
+ // update data
+ d.update = true;
+ d.updateColor = true;
+ d.layerName = layerName;
+ d.name = robotName;
+ if (c.a == 0 && c.b == 0 && c.r == 0 && c.g == 0)
+ d.color = VirtualRobot::VisualizationFactory::Color::None();
+ else
+ d.color = VirtualRobot::VisualizationFactory::Color(c.r, c.g, c.b, 1 - c.a);
+
+ d.active = true;
+}
+
+void DebugDrawerComponent::removeRobotVisu(const std::string &layerName, const std::string &robotName, const Ice::Current &)
+{
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + robotName + "__";
+ RobotData &d = accumulatedUpdateData.robots[entryName];
+ d.layerName = layerName;
+ d.name = robotName;
+ d.active = false;
+}
+
//todo: in some rare cases the mutex3D lock does not work and results in a broken coin timer setup. No updates of the scene will be drawn in this case
// -> check for a qt-thread solution
void DebugDrawerComponent::clearLayer(const std::string &layerName, const Ice::Current &)
@@ -1055,6 +1278,12 @@ void DebugDrawerComponent::updateVisualization()
}
accumulatedUpdateData.arrows.clear();
+ for (auto i = accumulatedUpdateData.robots.begin(); i != accumulatedUpdateData.robots.end(); i++)
+ {
+ drawRobot(i->second);
+ }
+ accumulatedUpdateData.robots.clear();
+
}
bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index f8cbc853b..004998149 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -181,6 +181,25 @@ public:
virtual void removeArrowVisu(const std::string &layerName, const std::string &arrowName, const ::Ice::Current& = ::Ice::Current());
virtual void removeArrowDebugLayerVisu(const std::string& arrowName, const ::Ice::Current& = ::Ice::Current());
+ /*!
+ * \brief setRobotVisu Initializes a robot visualization
+ * \param layerName The layer
+ * \param robotName The identifier of the robot
+ * \param robotFile The filename of the robot. The robot must be locally present in a project.
+ * \param armarxProject Additional armarx project that should be used to search the robot. Must be locally available and accessible through the armarx cmake search procedure.
+ */
+ virtual void setRobotVisu(const std::string &layerName, const std::string &robotName, const std::string &robotFile, const std::string &armarxProject, const ::Ice::Current& = ::Ice::Current());
+ virtual void updateRobotPose(const std::string &layerName, const std::string &robotName, const ::armarx::PoseBasePtr &globalPose, const ::Ice::Current& = ::Ice::Current());
+ virtual void updateRobotConfig(const std::string &layerName, const std::string &robotName, const std::map< std::string, float> &configuration, const ::Ice::Current& = ::Ice::Current());
+ /*!
+ * \brief updateRobotColor Colorizes the robot visualization
+ * \param layerName The layer
+ * \param robotName The robot identifyer
+ * \param c The draw color, if all is set to 0, the colorization is disabled (i.e. the original vizaulization shows up)
+ */
+ virtual void updateRobotColor(const std::string &layerName, const std::string &robotName, const armarx::DrawColor &c, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeRobotVisu(const std::string &layerName, const std::string &robotName, const ::Ice::Current& = ::Ice::Current());
+
virtual void clearLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
virtual void clearDebugLayer(const ::Ice::Current& = ::Ice::Current());
@@ -222,10 +241,11 @@ protected:
struct DrawData
{
- DrawData(){active=false;}
+ DrawData(){active=false;update=false;}
std::string layerName;
std::string name;
bool active;
+ bool update;
};
struct CoordData : public DrawData
@@ -280,6 +300,21 @@ protected:
float width;
VirtualRobot::VisualizationFactory::Color color;
};
+ struct RobotData : public DrawData
+ {
+ RobotData(){updateColor = false; updatePose = false; updateConfig = false;}
+ std::string robotFile;
+ std::string armarxProject;
+
+ bool updateColor;
+ VirtualRobot::VisualizationFactory::Color color;
+
+ bool updatePose;
+ Eigen::Matrix4f globalPose;
+
+ bool updateConfig;
+ std::map < std::string, float > configuration;
+ };
struct UpdateData
{
@@ -291,6 +326,7 @@ protected:
std::map<std::string, PointCloudData> pointcloud;
std::map<std::string, PolygonData> polygons;
std::map<std::string, ArrowData> arrows;
+ std::map<std::string, RobotData> robots;
};
UpdateData accumulatedUpdateData;
@@ -306,6 +342,7 @@ protected:
void drawPointCloud(const PointCloudData &d);
void drawPolygon(const PolygonData &d);
void drawArrow(const ArrowData &d);
+ void drawRobot(const RobotData &d);
void removeCoordSystem(const std::string& layerName, const std::string &name);
void removeLine(const std::string& layerName, const std::string &name);
@@ -315,6 +352,7 @@ protected:
void removePointCloud(const std::string& layerName, const std::string &name);
void removePolygon(const std::string& layerName, const std::string &name);
void removeArrow(const std::string& layerName, const std::string &name);
+ void removeRobot(const std::string& layerName, const std::string &name);
void setLayerVisibility(const std::string& layerName, bool visible);
@@ -332,6 +370,7 @@ protected:
std::map<std::string, SoSeparator*> addedPointCloudVisualizations;
std::map<std::string, SoSeparator*> addedPolygonVisualizations;
std::map<std::string, SoSeparator*> addedArrowVisualizations;
+ std::map<std::string, SoSeparator*> addedRobotVisualizations;
bool visible;
};
@@ -342,7 +381,9 @@ protected:
* \return The requested layer.
*/
Layer &requestLayer(const std::string& layerName);
+ VirtualRobot::RobotPtr requestRobot(const RobotData &d);
+ std::map < std::string, VirtualRobot::RobotPtr > activeRobots;
SoSeparator* coinVisu;
bool verbose;
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index bc0454a50..ea6a03bc5 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -27,6 +27,7 @@
#include <Core/interface/core/UserException.ice>
#include <Core/interface/core/BasicTypes.ice>
#include <RobotAPI/interface/core/PoseBase.ice>
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
module armarx
{
@@ -89,6 +90,25 @@ module armarx
void setPointCloudVisu(string layerName, string pointCloudName, DebugDrawerPointCloud pointCloud);
void setPolygonVisu(string layerName, string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
void setArrowVisu(string layerName, string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
+
+ /*!
+ * \brief setRobotVisu Initializes a robot visualization
+ * \param layerName The layer
+ * \param robotName The identifier of the robot
+ * \param robotFile The filename of the robot. The robot must be locally present in a project.
+ * \param armarxProject Additional armarx project that should be used to search the robot. Must be locally available and accessible through the armarx cmake search procedure.
+ */
+ void setRobotVisu(string layerName, string robotName, string robotFile, string armarxProject);
+ void updateRobotPose(string layerName, string robotName, PoseBase globalPose);
+ void updateRobotConfig(string layerName, string robotName, NameValueMap configuration);
+ /*!
+ * \brief updateRobotColor Colorizes the robot visualization
+ * \param layerName The layer
+ * \param robotName The robot identifyer
+ * \param c The draw color, if all is set to 0, the colorization is disabled (i.e. the original vizaulization shows up)
+ */
+ void updateRobotColor(string layerName, string robotName, DrawColor c);
+ void removeRobotVisu(string layerName, string robotName);
/*!
* \brief setPoseVisu draws on the "debug" layer
--
GitLab