diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 364414939a6068b14213764c090e72734e5bbfed..84ca152eab4300106f1de88084905f3234698976 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -80,7 +80,7 @@ void ForceTorqueObserver::onInitObserver()
offeringTopic("DebugDrawerUpdates");
auto sensorRobotNodeSplit = armarx::Split(getProperty<std::string>("SensorRobotNodeMapping").getValue(), ",");
- for (auto & elem : sensorRobotNodeSplit)
+ for (auto& elem : sensorRobotNodeSplit)
{
boost::trim(elem);
auto split = armarx::Split(elem, ":");
@@ -116,100 +116,120 @@ void ForceTorqueObserver::visualizerFunction()
return;
}
float maxTorque = getProperty<float>("MaxExpectedTorqueValue");
- //float torqueVisuDeathZone = getProperty<float>("TorqueVisuDeathZone");
+ float torqueVisuDeathZone = getProperty<float>("TorqueVisuDeathZone");
float arrowLength = getProperty<float>("MaxForceArrowLength").getValue();
float forceFactor = getProperty<float>("ForceVisualizerFactor").getValue();
auto channels = getAvailableChannels(false);
auto batchPrx = debugDrawer->ice_batchOneway();
- std::set<std::string> frameAlreadyReported;
- for (auto & channel : channels)
{
- try
+ ScopedLock lock(dataMutex);
+
+ std::set<std::string> frameAlreadyReported;
+ for (auto& channel : channels)
{
- // if (localRobot->hasRobotNode(channel.first))
+ try
{
- DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(getForceDatafield(channel.first, Ice::Current()));
-
- FramedDirectionPtr value = field->getDataField()->get<FramedDirection>();
- if (frameAlreadyReported.count(value->frame) > 0 || (value->frame != GlobalFrame && !value->frame.empty() && !localRobot->hasRobotNode(value->frame)))
- {
- continue;
- }
- frameAlreadyReported.insert(value->frame);
- auto force = value->toGlobal(localRobot);
- // ARMARX_INFO << deactivateSpam(5,channel.first) << "new force" << channel.first << " : " << force->toEigen();
-
- float forceMag = force->toEigen().norm() * forceFactor;
- Vector3Ptr pos = new Vector3(localRobot->getRobotNode(value->frame)->getGlobalPose());
- forceMag = std::min(1.0f, forceMag);
- batchPrx->setArrowVisu("Forces",
- "Force" + channel.first,
- pos,
- force,
- DrawColor {forceMag, 1.0f - forceMag, 0.0f, 0.5f},
- std::max(arrowLength * forceMag, 10.f),
- 3);
-
- field = DatafieldRefPtr::dynamicCast(getTorqueDatafield(channel.first, Ice::Current()));
- value = field->getDataField()->get<FramedDirection>();
- auto torque = value;//->toGlobal(localRobot);
-
-
- Eigen::Vector3f dirXglobal = FramedDirection(Eigen::Vector3f::UnitX(), torque->frame, torque->agent).toGlobalEigen(localRobot);
- Eigen::Vector3f dirYglobal = FramedDirection(Eigen::Vector3f::UnitY(), torque->frame, torque->agent).toGlobalEigen(localRobot);
- Eigen::Vector3f dirZglobal = FramedDirection(Eigen::Vector3f::UnitZ(), torque->frame, torque->agent).toGlobalEigen(localRobot);
- // ARMARX_INFO << channel.first << VAROUT(torque) << VAROUT(dirXglobal) << VAROUT(dirYglobal) << VAROUT(dirZglobal);
- // if (fabs(torque->x) > torqueVisuDeathZone)
- {
- batchPrx->setCircleArrowVisu("Torques",
- "TorqueX" + channel.first,
- pos,
- new Vector3(dirXglobal),
- 50,
- torque->x / maxTorque,
- 3 * std::max(1.0f, torque->x / maxTorque),
- DrawColor {1.0f, 0.0f, 0.0f, 0.5f}
- );
- }
- // if (fabs(torque->y) > torqueVisuDeathZone)
- {
- batchPrx->setCircleArrowVisu("Torques",
- "TorqueY" + channel.first,
- pos,
- new Vector3(dirYglobal),
- 50,
- torque->y / maxTorque,
- 3 * std::max(1.0f, torque->y / maxTorque),
- DrawColor {0.0f, 1.0f, 0.0f, 0.5f}
- );
- }
- // if (fabs(torque->z) > torqueVisuDeathZone)
+ // if (localRobot->hasRobotNode(channel.first))
{
- batchPrx->setCircleArrowVisu("Torques",
- "TorqueZ" + channel.first,
- pos,
- new Vector3(dirZglobal),
- 50,
- torque->z / maxTorque,
- 3 * std::max(1.0f, torque->z / maxTorque),
- DrawColor {0.0f, 0.0f, 1.0f, 0.5f}
- );
+ DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(getForceDatafield(channel.first, Ice::Current()));
+
+ FramedDirectionPtr value = field->getDataField()->get<FramedDirection>();
+ if (frameAlreadyReported.count(value->frame) > 0 || (value->frame != GlobalFrame && !value->frame.empty() && !localRobot->hasRobotNode(value->frame)))
+ {
+ continue;
+ }
+ frameAlreadyReported.insert(value->frame);
+ auto force = value->toGlobal(localRobot);
+ // ARMARX_INFO << deactivateSpam(5,channel.first) << "new force" << channel.first << " : " << force->toEigen();
+
+ float forceMag = force->toEigen().norm() * forceFactor;
+ Vector3Ptr pos = new Vector3(localRobot->getRobotNode(value->frame)->getGlobalPose());
+ forceMag = std::min(1.0f, forceMag);
+ batchPrx->setArrowVisu("Forces",
+ "Force" + channel.first,
+ pos,
+ force,
+ DrawColor {forceMag, 1.0f - forceMag, 0.0f, 0.5f},
+ std::max(arrowLength * forceMag, 10.f),
+ 3);
+
+ field = DatafieldRefPtr::dynamicCast(getTorqueDatafield(channel.first, Ice::Current()));
+ value = field->getDataField()->get<FramedDirection>();
+ auto torque = value;
+ // ARMARX_INFO << deactivateSpam(1, torque->frame) << "Reporting for " << channel.first << " in frame " << torque->frame;
+
+ Eigen::Vector3f dirXglobal = FramedDirection(Eigen::Vector3f::UnitX(), torque->frame, torque->agent).toGlobalEigen(localRobot);
+ Eigen::Vector3f dirYglobal = FramedDirection(Eigen::Vector3f::UnitY(), torque->frame, torque->agent).toGlobalEigen(localRobot);
+ Eigen::Vector3f dirZglobal = FramedDirection(Eigen::Vector3f::UnitZ(), torque->frame, torque->agent).toGlobalEigen(localRobot);
+ // ARMARX_INFO << channel.first << VAROUT(torque) << VAROUT(dirXglobal) << VAROUT(dirYglobal) << VAROUT(dirZglobal);
+ if (fabs(torque->x) > torqueVisuDeathZone)
+ {
+ batchPrx->setCircleArrowVisu("Torques",
+ "TorqueX" + channel.first,
+ pos,
+ new Vector3(dirXglobal),
+ 50,
+ torque->x / maxTorque,
+ 3 * std::max(1.0f, torque->x / maxTorque),
+ DrawColor {1.0f, 0.0f, 0.0f, 0.5f}
+ );
+ }
+ else
+ {
+ batchPrx->removeCircleVisu("Torques",
+ "TorqueX" + channel.first);
+ }
+ if (fabs(torque->y) > torqueVisuDeathZone)
+ {
+ batchPrx->setCircleArrowVisu("Torques",
+ "TorqueY" + channel.first,
+ pos,
+ new Vector3(dirYglobal),
+ 50,
+ torque->y / maxTorque,
+ 3 * std::max(1.0f, torque->y / maxTorque),
+ DrawColor {0.0f, 1.0f, 0.0f, 0.5f}
+ );
+ }
+ else
+ {
+ batchPrx->removeCircleVisu("Torques",
+ "TorqueY" + channel.first);
+
+ }
+ if (fabs(torque->z) > torqueVisuDeathZone)
+ {
+ batchPrx->setCircleArrowVisu("Torques",
+ "TorqueZ" + channel.first,
+ pos,
+ new Vector3(dirZglobal),
+ 50,
+ torque->z / maxTorque,
+ 3 * std::max(1.0f, torque->z / maxTorque),
+ DrawColor {0.0f, 0.0f, 1.0f, 0.5f}
+ );
+ }
+ else
+ {
+ batchPrx->removeCircleVisu("Torques",
+ "TorqueZ" + channel.first);
+
+ }
+
}
+ // else
+ // {
+ // // ARMARX_INFO << deactivateSpam(5,channel.first) << "No node name " << channel.first;
+ // }
}
+ catch (...)
+ {
- // else
- // {
- // // ARMARX_INFO << deactivateSpam(5,channel.first) << "No node name " << channel.first;
- // }
- }
- catch (...)
- {
-
+ }
}
}
batchPrx->ice_flushBatchRequests();
-
}
@@ -221,6 +241,7 @@ PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
void ForceTorqueObserver::updateRobot()
{
+ ScopedLock lock(dataMutex);
RemoteRobot::synchronizeLocalClone(localRobot, robot);
}
@@ -238,7 +259,7 @@ void ForceTorqueObserver::offerValue(const std::string& nodeName, const std::str
}
catch (...)
{
- ARMARX_INFO << "Creating force/torque fields";
+ ARMARX_INFO << "Creating force/torque fields for node " << nodeName << " with id " << id->getIdentifierStr();
if (!existsDataField(id->channelName, id->datafieldName))
{
if (!existsChannel(id->channelName))
@@ -280,7 +301,7 @@ void ForceTorqueObserver::offerValue(const std::string& nodeName, const std::str
}
-void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current&)
+void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current& c)
{
ScopedLock lock(dataMutex);
@@ -301,7 +322,7 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNo
id = new DataFieldIdentifier(getName(), channelName, "forces");
}
- offerValue(sensorNodeName, id->datafieldName, forces, id);
+ offerValue(sensorNodeName, id->datafieldName, forces, id, time);
}
if (torques)
@@ -315,7 +336,7 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNo
id = new DataFieldIdentifier(getName(), channelName, "torques");
}
- offerValue(sensorNodeName, id->datafieldName, torques, id);
+ offerValue(sensorNodeName, id->datafieldName, torques, id, time);
}
updateChannel(id->channelName);