6 #include <VirtualRobot/SceneObject.h>
7 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
8 #include <VirtualRobot/XML/RobotIO.h>
29 std::string fullFilename;
34 <<
"You specified the absolute path to the robot file:"
36 <<
"\nConsider specifying the containing ArmarX package and relative "
37 "data path instead to "
38 <<
"improve portability to other systems.";
45 <<
"Unable to find readable file for name: " <<
filename;
59 ARMARX_INFO <<
"Loading robot from " << fullFilename;
60 result = VirtualRobot::RobotIO::loadRobot(fullFilename, loadMode);
63 result->setThreadsafe(
false);
64 result->setPropagatingJointValuesEnabled(
true);
68 ARMARX_WARNING <<
"Could not load robot from file: " << fullFilename
69 <<
"\nReason: loadRobot() returned nullptr";
72 catch (std::exception
const& ex)
74 ARMARX_WARNING <<
"Could not load robot from file: " << fullFilename
75 <<
"\nReason: " << ex.what();
81 struct RobotInstancePool
86 std::vector<VirtualRobot::RobotPtr>
robots;
89 static std::vector<RobotInstancePool> robotCache;
92 getRobotFromCache(std::string
const&
project, std::string
const&
filename)
100 for (RobotInstancePool& instancePool : robotCache)
102 if (instancePool.project ==
project && instancePool.filename ==
filename)
105 if (instancePool.usedInstances < instancePool.robots.size())
110 result.robot = instancePool.robots[instancePool.usedInstances];
111 instancePool.usedInstances += 1;
120 if (instancePool.robots.size() > 0)
123 instancePool.robots.front();
124 float scaling = 1.0f;
125 bool preventCloningMeshesIfScalingIs1 =
true;
126 result.robot = robotToClone->clone(
127 nullptr, scaling, preventCloningMeshesIfScalingIs1);
130 instancePool.robots.push_back(result.robot);
131 instancePool.usedInstances += 1;
135 ARMARX_WARNING <<
"Encountered empty robot instance pool while trying "
136 "to clone new instance"
139 <<
"\nUsed instances: " << instancePool.usedInstances
140 <<
"\nRobots: " << instancePool.robots.size();
152 RobotInstancePool& instancePool = robotCache.emplace_back();
153 instancePool.project =
project;
155 instancePool.robots.push_back(result.robot);
156 instancePool.usedInstances = 1;
169 for (RobotInstancePool& instancePool : robotCache)
175 std::vector<VirtualRobot::RobotPtr>&
robots = instancePool.robots;
177 if (robotIter !=
robots.end())
182 if (instancePool.usedInstances > 0)
184 instancePool.usedInstances -= 1;
188 ARMARX_WARNING <<
"Expected there to be at least one used instance "
189 <<
"while trying to put robot instance back into the pool"
192 <<
"\nUsed instances: " << instancePool.usedInstances;
210 loaded = getRobotFromCache(element.project, element.filename);
215 <<
"Robot will not visualized since it could not be loaded."
216 <<
"\nID: " << element.id <<
"\nProject: " << element.project
217 <<
"\nFilename: " << element.filename;
223 if (robotChanged || drawStyleChanged)
243 for (
const auto& pair : element.jointValues)
245 std::string
const& nodeName = pair.first;
246 float newJointValue = pair.second;
247 VirtualRobot::RobotNodePtr robotNode = robot.getRobotNode(nodeName);
249 if (robotNode ==
nullptr)
254 const float oldJointValue = robotNode->getJointValue();
255 const float diff =
std::abs(newJointValue - oldJointValue);
256 const bool jointValuesChanged = diff > 0.001f;
257 if (jointValuesChanged)
261 robot.setJointValues(element.jointValues);
272 int numChildren =
node->getNumChildren();
273 for (
int i = 0; i < numChildren; i++)
275 SoSeparator* nodeSep =
static_cast<SoSeparator*
>(
node->getChild(i));
277 SoMaterial* m =
dynamic_cast<SoMaterial*
>(nodeSep->getChild(0));
284 auto color = element.color;
285 const float conv = 1.0f / 255.0f;
286 float a = color.a * conv;
287 SbColor coinColor(conv * color.r, conv * color.g, conv * color.b);
288 m->diffuseColor = coinColor;
289 m->ambientColor = coinColor;
290 m->transparency = 1.0f -
a;
291 m->setOverride(
true);
311 VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
312 if (drawStyle & data::ModelDrawStyle::COLLISION)
314 visuType = VirtualRobot::SceneObject::Collision;
317 node->removeAllChildren();
320 for (
size_t i = 0; i < robot.getRobotNodes().size(); ++i)
322 VirtualRobot::RobotNodePtr robNode = robot.getRobotNodes()[i];
323 SoSeparator* nodeSep =
new SoSeparator;
327 SoMaterial* nodeMat =
new SoMaterial;
328 nodeMat->setOverride(
false);
329 nodeSep->addChild(nodeMat);
331 auto robNodeVisu = robNode->getVisualization<VirtualRobot::CoinVisualization>(visuType);
334 SoNode* sepRobNode = robNodeVisu->getCoinVisualization();
338 nodeSep->addChild(sepRobNode);
342 node->addChild(nodeSep);