diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 6db1d3857cc8d841278dff4bf86c6ffe5e232e6f..0940cd7b20da5c3612ec4da86a2bcf4516d91e47 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -59,7 +59,8 @@ namespace armarx
void RobotStateComponent::onInitComponent()
{
-
+ robotStateTopicName = getProperty<std::string>("RobotStateReportingTopic").getValue();
+ offeringTopic(getProperty<std::string>("RobotStateReportingTopic"));
historyLength = getProperty<int>("HistoryLength").getValue();
{
boost::unique_lock<SharedMutex> lock(historyMutex);
@@ -131,13 +132,14 @@ namespace armarx
ARMARX_VERBOSE << "Node: " << node->getName() << endl;
}
}*/
- _sharedRobotServant = new SharedRobotServant(this->_synchronized, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()));
- _sharedRobotServant->ref();
}
void RobotStateComponent::onConnectComponent()
{
+ robotStateListenerPrx = getTopic<RobotStateListenerInterfacePrx>(getProperty<std::string>("RobotStateReportingTopic"));
+ _sharedRobotServant = new SharedRobotServant(this->_synchronized, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), robotStateListenerPrx);
+ _sharedRobotServant->ref();
_sharedRobotServant->setRobotStateComponent(RobotStateComponentInterfacePrx::uncheckedCast(getProxy()));
this->_synchronizedPrx = SharedRobotInterfacePrx::uncheckedCast(getObjectAdapter()->addWithUUID(_sharedRobotServant));
@@ -173,7 +175,7 @@ namespace armarx
auto clone = this->_synchronized->clone(_synchronized->getName());
- SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()));
+ SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), NULL);
p->setTimestamp(IceUtil::Time::microSecondsDouble(_sharedRobotServant->getTimestamp()->timestamp));
auto result = getObjectAdapter()->addWithUUID(p);
// virtal robot clone is buggy -> set global pose here
@@ -197,7 +199,7 @@ namespace armarx
}
clone->setJointValues(config.jointMap);
- SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()));
+ SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), NULL);
p->setTimestamp(IceUtil::Time::microSecondsDouble(time));
auto result = getObjectAdapter()->addWithUUID(p);
// virtal robot clone is buggy -> set global pose here
@@ -263,7 +265,7 @@ namespace armarx
double influencePrev = 1.0f - (double)(time - prevIt->first) / deltaT;
// ARMARX_INFO_S << "Interpolating: " << time << " prev: " << time - prevIt->first << " next: " << it->first - time << VAROUT(influenceNext) << VAROUT(influencePrev) << " sum: " << (influenceNext + influencePrev);
auto jointIt = prevIt->second.jointMap.begin();
- for (auto& joint : config.jointMap)
+ for (auto & joint : config.jointMap)
{
joint.second = joint.second * influenceNext + jointIt->second * influencePrev;
jointIt++;
@@ -330,6 +332,8 @@ namespace armarx
{
history.erase(history.begin());
}
+
+ robotStateListenerPrx->reportJointValues(jointAngles, timestamp, aValueChanged);
}
std::string RobotStateComponent::getRobotFilename(const Ice::Current&) const
@@ -348,7 +352,7 @@ namespace armarx
auto packages = armarx::Application::GetProjectDependencies();
packages.push_back(Application::GetProjectName());
- for (const std::string& projectName : packages)
+ for (const std::string & projectName : packages)
{
if (projectName.empty())
{
@@ -412,9 +416,14 @@ namespace armarx
return robotNodeSetName;
}
+ string RobotStateComponent::getRobotStateTopicName(const Current&) const
+ {
+ return robotStateTopicName;
+ }
}
+
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 781da9a4d60599c46a739bce8803956d5e13f2ea..949b8951c881d15b09910876cf0ccb410244333b 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -53,6 +53,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<std::string>("RobotStateReportingTopic", "RobotStateUpdates", "Name of the topic on which updates of the robot state are reported.");
defineOptionalProperty<int>("HistoryLength", 10000, "Number of entries in the robot state history");
defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
}
@@ -117,6 +118,8 @@ namespace armarx
*/
virtual std::string getRobotName(const Ice::Current&) const;
+ std::string getRobotStateTopicName(const Ice::Current&) const;
+
/**
*
* \return The name of this robot instance.
@@ -167,7 +170,8 @@ namespace armarx
SharedRobotInterfacePrx _synchronizedPrx;
SharedRobotServantPtr _sharedRobotServant;
VirtualRobot::RobotPtr _synchronized;
-
+ RobotStateListenerInterfacePrx robotStateListenerPrx;
+ std::string robotStateTopicName;
std::string robotFile;
std::string relativeRobotFile;
RobotStateObserverPtr robotStateObs;
@@ -179,6 +183,7 @@ namespace armarx
std::string robotNodeSetName;
float robotModelScaling;
+
};
}
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
index 1a1f775407ce20b70f25c6bae51d40da6ba5482c..ec0574032b5accb9e00e1df6957f9b6d43005d50 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
@@ -308,9 +308,10 @@ namespace armarx
// SharedRobotServant
///////////////////////////////
- SharedRobotServant::SharedRobotServant(RobotPtr robot, RobotStateComponentInterfacePrx robotStateComponent)
+ SharedRobotServant::SharedRobotServant(RobotPtr robot, RobotStateComponentInterfacePrx robotStateComponent, RobotStateListenerInterfacePrx robotStateListenerPrx)
: _robot(robot),
- robotStateComponent(robotStateComponent)
+ robotStateComponent(robotStateComponent),
+ robotStateListenerPrx(robotStateListenerPrx)
{
#ifdef VERBOSE
ARMARX_WARNING_S << "construct " << this << flush;
@@ -491,7 +492,13 @@ namespace armarx
void SharedRobotServant::setGlobalPose(const armarx::PoseBasePtr& pose, const Current&)
{
WriteLockPtr lock = _robot->getWriteLock();
- _robot->setGlobalPose(PosePtr::dynamicCast(pose)->toEigen(), true);
+ Eigen::Matrix4f newPose = PosePtr::dynamicCast(pose)->toEigen();
+ if (robotStateListenerPrx)
+ {
+ Eigen::Matrix4f oldPose = _robot->getGlobalPose();
+ robotStateListenerPrx->reportGlobalRobotRootPose(new FramedPose(newPose, GlobalFrame, ""), TimeUtil::GetTime().toMicroSeconds(), !oldPose.isApprox(newPose));
+ }
+ _robot->setGlobalPose(newPose, true);
}
float SharedRobotServant::getScaling(const Current&)
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.h b/source/RobotAPI/components/RobotState/SharedRobotServants.h
index e0877564f52d06c1e0b1968d54287f3cfd126f64..892160f7c0dec61e31327a986b6f447e0bfe0d88 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.h
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.h
@@ -121,7 +121,7 @@ namespace armarx
public SharedObjectBase
{
public:
- SharedRobotServant(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStateComponent);
+ SharedRobotServant(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStateComponent, RobotStateListenerInterfacePrx robotStateListenerPrx);
~SharedRobotServant();
void setRobotStateComponent(RobotStateComponentInterfacePrx robotStateComponent);
virtual SharedRobotNodeInterfacePrx getRobotNode(const std::string& name, const Ice::Current& current = Ice::Current());
@@ -154,6 +154,7 @@ namespace armarx
std::map<std::string, SharedRobotNodeInterfacePrx> _cachedNodes;
IceUtil::Time updateTimestamp;
RobotStateComponentInterfacePrx robotStateComponent;
+ RobotStateListenerInterfacePrx robotStateListenerPrx;
};
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
index f2a851d898e27b17e211f8bfb04126c354fbe2dc..764ff8214e825d22975f2a61274d4cbfa5c6e7f3 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
@@ -154,7 +154,7 @@ void RobotViewerWidgetController::onInitComponent()
void RobotViewerWidgetController::onConnectComponent()
{
robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
-
+ usingTopic(robotStateComponentPrx->getRobotStateTopicName());
if (robotVisu)
{
robotVisu->removeAllChildren();
@@ -678,15 +678,15 @@ void RobotViewerWidgetController::updateRobotVisu()
return;
}
- try
- {
- RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
- }
- catch (...)
- {
- ARMARX_INFO << deactivateSpam(5) << "Robot synchronization failed";
- return;
- }
+ // try
+ // {
+ // RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
+ // }
+ // catch (...)
+ // {
+ // ARMARX_INFO << deactivateSpam(5) << "Robot synchronization failed";
+ // return;
+ // }
Eigen::Matrix4f gp = robot->getGlobalPose();
QString roboInfo("Robot Pose (global): pos: ");
@@ -727,3 +727,30 @@ QIcon armarx::RobotViewerWidgetController::getWidgetIcon() const
{
return QIcon(":icons/armarx-gui.png");
}
+
+
+void armarx::RobotViewerWidgetController::reportGlobalRobotRootPose(const FramedPoseBasePtr& pose, Ice::Long timestamp, bool poseChanged, const Ice::Current&)
+{
+ boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ if (!robotStateComponentPrx || !robot)
+ {
+ return;
+ }
+ Eigen::Matrix4f newPose = PosePtr::dynamicCast(pose)->toEigen();
+
+ if (!robot->getGlobalPose().isApprox(newPose))
+ {
+ robot->setGlobalPose(newPose);
+ }
+}
+
+void armarx::RobotViewerWidgetController::reportJointValues(const NameValueMap& jointAngles, Ice::Long, bool aValueChanged, const Ice::Current&)
+{
+ boost::recursive_mutex::scoped_lock lock(*mutex3D);
+
+ if (!robotStateComponentPrx || !robot || !aValueChanged)
+ {
+ return;
+ }
+ robot->setJointValues(jointAngles);
+}
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
index f205b3d0d8f8d2b7835bd73d1c296b6c5ae4d660..1ef7ff3e7806f7eb80a1a73e502ae37859bf5433 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
@@ -86,7 +86,8 @@ namespace armarx
Proxy | RobotStateComponent | Yes | The name of the robot state component.
*/
class RobotViewerWidgetController :
- public ArmarXComponentWidgetController
+ public ArmarXComponentWidgetController,
+ public RobotStateListenerInterface
{
Q_OBJECT
public:
@@ -177,6 +178,11 @@ namespace armarx
// ArmarXWidgetController interface
public:
QIcon getWidgetIcon() const;
+
+ // RobotStateListenerInterface interface
+ public:
+ void reportGlobalRobotRootPose(const FramedPoseBasePtr& pose, Ice::Long timestamp, bool poseChanged, const Ice::Current&);
+ void reportJointValues(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current&);
};
typedef boost::shared_ptr<RobotViewerWidgetController> RobotViewerGuiPluginPtr;
}
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 3a5d71290ee64ea853565f0836dcab8ae8cd7d11..9ca15ba68d1e85666cebfd609ac821b4eb2fb7d9 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -234,6 +234,9 @@ module armarx
["cpp:const"]
idempotent string getRobotName() throws NotInitializedException;
+ ["cpp:const"]
+ idempotent string getRobotStateTopicName() throws NotInitializedException;
+
/**
* @return The scaling of the robot model represented by this component.
@@ -249,6 +252,12 @@ module armarx
["cpp:const"]
idempotent string getRobotNodeSetName() throws NotInitializedException;
};
+
+ interface RobotStateListenerInterface
+ {
+ void reportGlobalRobotRootPose(FramedPoseBase globalPose, long timestamp, bool poseChanged);
+ void reportJointValues(NameValueMap jointAngles, long timestamp, bool aValueChanged);
+ };
};
#endif