Skip to content
Snippets Groups Projects
Commit a32c415a authored by Mirko Waechter's avatar Mirko Waechter
Browse files

Adapted to ForceTorque interface changes

parent 4b7cc3d3
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/components/units/ForceTorqueObserver.cpp
+ 55
− 154
View file @ a32c415a
......@@ -44,14 +44,7 @@ void ForceTorqueObserver::onInitObserver()
offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
// offerConditionCheck("magnitudesmaller", new ConditionCheckSmaller());
// register all channels
// offerChannel(FORCETORQUEVECTORS,"Force and Torque vectors on specific parts of the robot.");
// offerChannel(RAWFORCE,"Forces on specific parts of the robot.");
// offerChannel(OFFSETFORCE,"Force Torques of specific parts of the robot with an offset.");
// offerChannel(FILTEREDFORCE,"Gaussian filtered force torques of specific parts of the robot.");
// offerChannel(RAWTORQUE,"Torques on specific parts of the robot.");
}
......@@ -61,175 +54,83 @@ void ForceTorqueObserver::onConnectObserver()
}
//void ForceTorqueObserver::reportForceRaw(const FramedVector3Map &newForces, const Ice::Current &)
//{
//}
//void ForceTorqueObserver::reportForceWithOffset(const FramedVector3Map &newForces, const Ice::Current &)
//{
// FramedVector3Map::const_iterator it = newForces.begin();
// for(; it != newForces.end(); it++)
// {
// FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
// if(!existsDataField(OFFSETFORCE, it->first))
// offerDataFieldWithDefault(OFFSETFORCE, it->first, Variant(it->second), "3D vector for Force Torques of " + it->first);
// else
// setDataField(OFFSETFORCE, it->first, Variant(it->second));
PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new ForceTorqueObserverPropertyDefinitions(
getConfigIdentifier()));
}
// if(!existsChannel(it->first))
// {
// offerChannel(it->first,"Force on " + it->first);
// offerDataFieldWithDefault(it->first,"force_x", Variant(vec->x), "Force on X axis");
// offerDataFieldWithDefault(it->first,"force_y", Variant(vec->y), "Force on Y axis");
// offerDataFieldWithDefault(it->first,"force_z", Variant(vec->z), "Force on Z axis");
// }
// else
// {
// setDataField(it->first,"force_x", Variant(vec->x));
// setDataField(it->first,"force_y", Variant(vec->y));
// setDataField(it->first,"force_z", Variant(vec->z));
// }
// }
//}
void ForceTorqueObserver::offerValue(std::string channelName, const std::string &type, FramedVector3BasePtr value)
{
if(!existsChannel(channelName))
{
offerChannel(channelName, "Torque vectors on specific parts of the robot.");
}
void ForceTorqueObserver::reportSensorValues(const ::armarx::FramedVector3BasePtr &forces, const ::armarx::FramedVector3BasePtr &torques, const Ice::Current &)
{
ScopedLock lock(dataMutex);
/*
FramedVector3Map::const_iterator it = newValues.begin();
if(!existsChannel(type))
offerChannel(type, "Force and Torque vectors on specific parts of the robot.");
for(; it != newValues.end(); it++)
FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(value);
std::string identifier = type;
if(channelName != value->frame)
{
identifier += "_" + value->frame;
}
FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
std::string identifier = it->first + "_" + type + "_in_frame_" + vec->frame;
if(!existsDataField(type, identifier))
offerDataFieldWithDefault(type, identifier, Variant(it->second), "3D vector for Force Torques of " + it->first);
else
setDataField(type, identifier, Variant(it->second));
if(!existsChannel(identifier))
{
offerChannel(identifier,type + " on " + it->first);
offerDataFieldWithDefault(identifier,type + "_x", Variant(vec->x), type + " on X axis");
offerDataFieldWithDefault(identifier,type + "_y", Variant(vec->y), type + " on Y axis");
offerDataFieldWithDefault(identifier,type + "_z", Variant(vec->z), type + " on Z axis");
offerDataFieldWithDefault(identifier,type + "_frame", Variant(vec->frame), "Frame of " + type);
}
else
{
setDataField(identifier,type + "_x", Variant(vec->x));
setDataField(identifier,type + "_y", Variant(vec->y));
setDataField(identifier,type + "_z", Variant(vec->z));
setDataField(identifier,type + "_frame", Variant(vec->frame));
}
updateChannel(identifier);
}*/
}
if(!existsDataField(channelName, identifier))
{
offerDataFieldWithDefault(channelName, identifier, Variant(value), "3D vector for " + type + " of " + channelName);
}
else
{
setDataField(channelName, identifier, Variant(value));
}
updateChannel(channelName);
void ForceTorqueObserver::reportSensorForceValues(const ::armarx::FramedVector3BasePtr &newForces, const ::Ice::Current &)
{
ScopedLock lock(dataMutex);
/*FramedVector3Map::const_iterator it = newForces.begin();
std::string type = "force";
if(!existsChannel(type))
channelName += "_pod";
if(!existsChannel(channelName))
{
offerChannel(type, "Force vectors on specific parts of the robot.");
offerChannel(channelName, type + " on " + channelName);
offerDataFieldWithDefault(channelName, type + "_x", Variant(vec->x), type + " on X axis");
offerDataFieldWithDefault(channelName, type + "_y", Variant(vec->y), type + " on Y axis");
offerDataFieldWithDefault(channelName, type + "_z", Variant(vec->z), type + " on Z axis");
offerDataFieldWithDefault(channelName, type + "_frame", Variant(vec->frame), "Frame of " + value);
}
for(; it != newForces.end(); it++)
else
{
setDataField(channelName, type + "_x", Variant(vec->x));
setDataField(channelName, type + "_y", Variant(vec->y));
setDataField(channelName, type + "_z", Variant(vec->z));
setDataField(channelName, type + "_frame", Variant(vec->frame));
}
FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
std::string identifier = it->first + "_" + type + "_in_frame_" + vec->frame;
updateChannel(channelName);
if(!existsDataField(type, identifier))
{
offerDataFieldWithDefault(type, identifier, Variant(it->second), "3D vector for Force of " + it->first);
}
else
{
setDataField(type, identifier, Variant(it->second));
}
if(!existsChannel(identifier))
{
offerChannel(identifier, type + " on " + it->first);
offerDataFieldWithDefault(identifier, type + "_x", Variant(vec->x), type + " on X axis");
offerDataFieldWithDefault(identifier, type + "_y", Variant(vec->y), type + " on Y axis");
offerDataFieldWithDefault(identifier, type + "_z", Variant(vec->z), type + " on Z axis");
offerDataFieldWithDefault(identifier, type + "_frame", Variant(vec->frame), "Frame of " + type);
}
else
{
setDataField(identifier, type + "_x", Variant(vec->x));
setDataField(identifier, type + "_y", Variant(vec->y));
setDataField(identifier, type + "_z", Variant(vec->z));
setDataField(identifier, type + "_frame", Variant(vec->frame));
}
updateChannel(identifier);
}*/
}
void ForceTorqueObserver::reportSensorTorqueValues(const ::armarx::FramedVector3BasePtr &newTorques, const ::Ice::Current &)
void armarx::ForceTorqueObserver::reportSensorValues(const std::string &sensorNodeName, const FramedVector3BasePtr &forces, const FramedVector3BasePtr &torques, const Ice::Current &)
{
ScopedLock lock(dataMutex);
/*FramedVector3Map::const_iterator it = newTorques.begin();
std::string type = "torque";
if(!existsChannel(type))
std::string channelName;
if(forces)
{
offerChannel(type, "Torque vectors on specific parts of the robot.");
if(forces->frame == sensorNodeName)
channelName = forces->frame;
else
channelName = sensorNodeName + "_" + forces->frame;
std::string type = "forces";
offerValue(channelName, type, forces);
}
for(; it != newTorques.end(); it++)
if(torques)
{
FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
std::string identifier = it->first + "_" + type + "_in_frame_" + vec->frame;
if(!existsDataField(type, identifier))
{
offerDataFieldWithDefault(type, identifier, Variant(it->second), "3D vector for Torque of " + it->first);
}
else
{
setDataField(type, identifier, Variant(it->second));
}
if(!existsChannel(identifier))
{
offerChannel(identifier, type + " on " + it->first);
offerDataFieldWithDefault(identifier, type + "_x", Variant(vec->x), type + " on X axis");
offerDataFieldWithDefault(identifier, type + "_y", Variant(vec->y), type + " on Y axis");
offerDataFieldWithDefault(identifier, type + "_z", Variant(vec->z), type + " on Z axis");
offerDataFieldWithDefault(identifier, type + "_frame", Variant(vec->frame), "Frame of " + type);
}
if(torques->frame == sensorNodeName)
channelName = torques->frame;
else
{
setDataField(identifier, type + "_x", Variant(vec->x));
setDataField(identifier, type + "_y", Variant(vec->y));
setDataField(identifier, type + "_z", Variant(vec->z));
setDataField(identifier, type + "_frame", Variant(vec->frame));
}
updateChannel(identifier);
}*/
}
PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new ForceTorqueObserverPropertyDefinitions(
getConfigIdentifier()));
channelName = sensorNodeName + "_" + torques->frame;
std::string type = "torques";
offerValue(channelName, type, torques);
}
}
source/RobotAPI/components/units/ForceTorqueObserver.h
+ 3
− 3
View file @ a32c415a
......@@ -37,9 +37,7 @@ namespace armarx
void onInitObserver();
void onConnectObserver();
void reportSensorValues(const ::armarx::FramedVector3BasePtr &forces, const ::armarx::FramedVector3BasePtr &torques, const ::Ice::Current& = ::Ice::Current());
void reportSensorForceValues(const ::armarx::FramedVector3BasePtr &forces, const ::Ice::Current& = ::Ice::Current());
void reportSensorTorqueValues(const ::armarx::FramedVector3BasePtr &torques, const ::Ice::Current& = ::Ice::Current());
virtual void reportSensorValues(const std::string &sensorNodeName, const FramedVector3BasePtr &forces, const FramedVector3BasePtr &torques, const Ice::Current &);
/**
* @see PropertyUser::createPropertyDefinitions()
......@@ -49,6 +47,8 @@ namespace armarx
armarx::Mutex dataMutex;
std::string topicName;
void offerValue(std::string channelName, const std::string &type, FramedVector3BasePtr value);
};
}
......
source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
+ 1
− 2
View file @ a32c415a
......@@ -64,8 +64,7 @@ void ForceTorqueUnitSimulation::onExitForceTorqueUnit()
void ForceTorqueUnitSimulation::simulationFunction()
{
listenerPrx->reportSensorForceValues(forces);
listenerPrx->reportSensorTorqueValues(torques);
listenerPrx->reportSensorValues(getProperty<std::string>("SensorName").getValue(), forces, torques);
//listenerPrx->reportSensorValues(forces);
//listenerPrx->reportSensorValues(torques);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment