diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index f0a9068ef0abd9d89876cc025fe6358d7c9ecd53..dbcf3ff490fd158caf471b3308bbaa8f89da98d9 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -75,8 +75,8 @@ namespace armarx
//remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
robotName = robotStateComponentPrx->getRobotName();
- localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
- jointExistenceCheckRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
+ localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx, robotStateComponentPrx->getRobotFilename(), robotStateComponentPrx->getArmarXPackages());
+ jointExistenceCheckRobot = RemoteRobot::createLocalClone(robotStateComponentPrx, robotStateComponentPrx->getRobotFilename(), robotStateComponentPrx->getArmarXPackages());
localReportRobot = localRobot->clone(localRobot->getName());
@@ -114,7 +114,7 @@ namespace armarx
}
else
{
- ARMARX_ERROR << "Could not find node set with name: " << name;
+ ARMARX_ERROR << "Could not find node with name: " << name;
}
}
}
@@ -1319,6 +1319,7 @@ void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles,
tempMap.erase("timestamp");
localReportRobot->setJointValues(tempMap);
}
+ ARMARX_INFO << deactivateSpam(1) << jointAngles;
//IceUtil::Time start = IceUtil::Time::now();
for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
@@ -1326,6 +1327,7 @@ void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles,
const std::string& tcpName = node->getName();
const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robotName);
+ ARMARX_INFO << deactivateSpam(1, tcpName) << tcpName << " New Pose: " << tcpPoses[tcpName]->output();
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index eeea47731b80b4c4108789f331f9d877ee789805..14bbb6cd3bab106b126b4a7c5717fcd9ba288e27 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -48,7 +48,6 @@ namespace armarx
defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit Proxy");
defineOptionalProperty<std::string>("RobotStateTopicName", "RobotState", "Name of the RobotComponent State topic.");
defineOptionalProperty<float>("MaxJointVelocity", 30.f / 180 * 3.141, "Maximal joint velocity in rad/sec");
- defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
defineOptionalProperty<int>("CycleTime", 30, "Cycle time of the tcp control in ms");
// defineOptionalProperty<float>("MaximumCommandDelay", 20000, "Delay after which the TCP Control unit releases itself if no new velocity have been set.");
defineOptionalProperty<std::string>("TCPsToReport", "", "comma seperated list of nodesets' endeffectors, which poses and velocities that should be reported. * for all, empty for none");
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index f640b6f7d5ba4c2d724271d1d967fe966f102e70..1afddfda30e6e7d413e23b1492dc76d45441e475 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -8,6 +8,8 @@
#include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
#include <VirtualRobot/Nodes/RobotNodeRevoluteFactory.h>
#include <Eigen/Geometry>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj << endl;
#define DMES(Obj) ;
@@ -325,16 +327,18 @@ namespace armarx
return robo;
}
- VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string& filename)
+ VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string& filename, const Ice::StringSeq packages)
{
- return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename, robotStatePrx->getScaling());
+ return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename, robotStatePrx->getScaling(), packages);
}
- RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string& filename, float scaling)
+ RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, std::string filename, float scaling, const Ice::StringSeq packages)
{
+ RobotPtr result;
+
boost::recursive_mutex::scoped_lock cloneLock(m);
ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
- VirtualRobot::RobotPtr result;
+
if (!sharedRobotPrx)
{
@@ -355,6 +359,33 @@ namespace armarx
}
else
{
+ StringList includePaths;
+ for (const std::string & projectName : packages)
+ {
+ if (projectName.empty())
+ {
+ continue;
+ }
+
+ CMakePackageFinder project(projectName);
+ StringList projectIncludePaths;
+ auto pathsString = project.getDataDir();
+ ARMARX_DEBUG_S << "Data paths of ArmarX package " << projectName << ": " << pathsString;
+ boost::split(projectIncludePaths,
+ pathsString,
+ boost::is_any_of(";,"),
+ boost::token_compress_on);
+ ARMARX_DEBUG_S << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
+ includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+
+ }
+
+
+ if (!ArmarXDataPath::getAbsolutePath(filename, filename, includePaths))
+ {
+ ARMARX_ERROR_S << "Could find robot file " << filename;
+ return result;
+ }
result = RobotIO::loadRobot(filename);
if (!result)
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index eaab284f832c140d4b594ee597e86e7ae0fd6969..c4a6134fcbbf394d6a27f5d22d592ed4b6e30ae2 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -179,14 +179,15 @@ namespace armarx
/*!
Creates a local robot that is synchronized once but not updated any more. You can use the synchronizeLocalClone method to update the joint values.
- Note that only the kinematic structure is cloned, but the visualization files, collision mdoels, end effectors, etc are not copied.
+ Note that only the kinematic structure is cloned, but the visualization files, collision models, end effectors, etc are not copied.
This means you can use this model for kinematic computations (e.g. coordinate transformations) but not for advanced features like collision detection.
In order to get a fully featured robot model you can pass a filename to VirtualRobot::Robot,
but then you need to make sure that the loaded model is identical to the model of the remote robot (otherwise errors will occur).
+ In the packages parameter you can pass in ArmarX packages, in which the robot file model might be in.
*/
- static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename = std::string());
+ static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename = std::string(), const Ice::StringSeq packages = Ice::StringSeq());
- static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const std::string& filename = std::string(), float scaling = 1.0f);
+ static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, std::string filename = std::string(), float scaling = 1.0f, const Ice::StringSeq packages = Ice::StringSeq());
/*!
Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.