From d68c47a24c6aa0cf96b1e11be722d3c948425d06 Mon Sep 17 00:00:00 2001
From: zhou <derekyou.zhou@gmail.com>
Date: Sun, 13 Mar 2016 14:26:36 +0100
Subject: [PATCH] Scaling support for RobotStateComponent (also fixes issues
with poses of scaled models in the world)
---
.../components/RobotState/RobotStateComponent.cpp | 14 ++++++++++++++
.../components/RobotState/RobotStateComponent.h | 5 ++++-
source/RobotAPI/interface/core/RobotState.ice | 8 ++++++++
.../libraries/core/remoterobot/RemoteRobot.cpp | 10 ++++++++--
.../libraries/core/remoterobot/RemoteRobot.h | 2 +-
5 files changed, 35 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 4d618fa97..d967974ff 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -68,6 +68,14 @@ namespace armarx
this->_synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
_synchronized->setName(getProperty<std::string>("AgentName").getValue());
+ robotModelScaling = getProperty<float>("RobotModelScaling").getValue();
+ ARMARX_INFO << "scale factor: " << robotModelScaling;
+ if (robotModelScaling != 1.0f)
+ {
+ ARMARX_INFO << "Scaling robot model with scale factor " << robotModelScaling;
+ _synchronized = _synchronized->clone(_synchronized->getName(), _synchronized->getCollisionChecker(), robotModelScaling);
+ }
+
if (this->_synchronized)
{
ARMARX_VERBOSE << "Loaded robot from file " << robotFile << ". Robot name: " << this->_synchronized->getName();
@@ -101,6 +109,7 @@ namespace armarx
}
usingTopic(robotNodeSetName + "State");
+
/*VirtualRobot::RobotNodeSetPtr pns = this->_synchronized->getRobotNodeSet("Platform");
if (pns)
{
@@ -198,6 +207,11 @@ namespace armarx
return relativeRobotFile;
}
+ float RobotStateComponent::getScaling(const Ice::Current&) const
+ {
+ return robotModelScaling;
+ }
+
std::vector<string> RobotStateComponent::getArmarXPackages(const Current&) const
{
std::vector<string> result;
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index f5c1d1434..f1a3b2d82 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -51,6 +51,7 @@ namespace armarx
defineRequiredProperty<std::string>("RobotNodeSetName", "Set of nodes that is controlled by the KinematicUnit");
defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
+ defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
}
};
@@ -126,6 +127,8 @@ namespace armarx
return "RobotStateComponent";
}
void setRobotStateObserver(RobotStateObserverPtr observer);
+
+ float getScaling(const Ice::Current&) const;
protected:
/**
* Load and create a VirtualRobot::Robot instance from the RobotFileName
@@ -162,7 +165,7 @@ namespace armarx
std::string robotNodeSetName;
-
+ float robotModelScaling;
};
}
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index c65d1ee36..0e91dc6c0 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -157,6 +157,14 @@ module armarx
["cpp:const"]
idempotent string getRobotName() throws NotInitializedException;
+
+ /**
+ * @return The scaling of the robot model represented by this component.
+ *
+ */
+ ["cpp:const"]
+ idempotent float getScaling();
+
/**
* @return The name of the robot node set that is represented by this component.
*
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 7cb4951b4..93c004495 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -322,10 +322,10 @@ namespace armarx
VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string& filename)
{
- return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename);
+ return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename, robotStatePrx->getScaling());
}
- RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string& filename)
+ RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string& filename, float scaling)
{
boost::recursive_mutex::scoped_lock cloneLock(m);
ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
@@ -357,6 +357,12 @@ namespace armarx
ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl;
return result;
}
+
+ if (scaling != 1.0f)
+ {
+ ARMARX_INFO_S << "Scaling robot to " << scaling;
+ result = result->clone(result->getName(), result->getCollisionChecker(), scaling);
+ }
}
synchronizeLocalClone(result, sharedRobotPrx);
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index 64bca7236..5d33c4bb8 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -187,7 +187,7 @@ namespace armarx
*/
static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename = std::string());
- static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const std::string& filename = std::string());
+ static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const std::string& filename = std::string(), float scaling = 1.0f);
/*!
Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.
--
GitLab