Skip to content

Commit

Permalink
Update spherical joint variables name in XsensSuit device
Browse files Browse the repository at this point in the history
  • Loading branch information
@yeshasvitirupachuri
yeshasvitirupachuri committed Apr 4, 2019
1 parent e69b83a commit 4f55e8c
Showing 1 changed file with 22 additions and 22 deletions.
44 changes: 22 additions & 22 deletions devices/XsensSuit/src/XsensSuit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ class XsensSuit::XsensSuitImpl
std::map<std::string, driverToDeviceSensors<XsensVirtualLinkKinSensor>>
virtualLinkKinSensorsMap;
std::map<std::string, driverToDeviceSensors<XsensVirtualSphericalJointKinSensor>>
virtualJointKinSensorsMap;
virtualSphericalJointKinSensorsMap;

// ------------------------
// Custom utility functions
Expand Down Expand Up @@ -515,16 +515,16 @@ class XsensSuit::XsensSuitImpl::XsensVirtualSphericalJointKinSensor
// -----------------------------------------
bool getJointAnglesAsRPY(Vector3& angleAsRPY) const override
{
if (m_suitImpl->virtualJointKinSensorsMap.find(this->m_name)
== m_suitImpl->virtualJointKinSensorsMap.end()) {
if (m_suitImpl->virtualSphericalJointKinSensorsMap.find(this->m_name)
== m_suitImpl->virtualSphericalJointKinSensorsMap.end()) {
yError() << logPrefix << "Sensor" << this->m_name << "NOT found";
angleAsRPY.fill(0.0);
return false;
}

// Retrieve data sample directly form XSens Driver
const xsensmvn::JointData jointData = m_suitImpl->driver->getJointDataSample().data.at(
m_suitImpl->virtualJointKinSensorsMap.at(this->m_name).driverIndex);
m_suitImpl->virtualSphericalJointKinSensorsMap.at(this->m_name).driverIndex);

// TODO: This should be guaranteed, runtime check should be removed
// Check if suit sensor and driver data sample have the same name
Expand All @@ -542,16 +542,16 @@ class XsensSuit::XsensSuitImpl::XsensVirtualSphericalJointKinSensor

bool getJointVelocities(Vector3& velocities) const override
{
if (m_suitImpl->virtualJointKinSensorsMap.find(this->m_name)
== m_suitImpl->virtualJointKinSensorsMap.end()) {
if (m_suitImpl->virtualSphericalJointKinSensorsMap.find(this->m_name)
== m_suitImpl->virtualSphericalJointKinSensorsMap.end()) {
yError() << logPrefix << "Sensor" << this->m_name << "NOT found";
velocities.fill(0.0);
return false;
}

// Retrieve data sample directly form XSens Driver
const auto jointData = m_suitImpl->driver->getJointDataSample().data.at(
m_suitImpl->virtualJointKinSensorsMap.at(this->m_name).driverIndex);
m_suitImpl->virtualSphericalJointKinSensorsMap.at(this->m_name).driverIndex);

// TODO: This should be guaranteed, runtime check should be removed
// Check if suit sensor and driver data sample have the same name
Expand All @@ -569,16 +569,16 @@ class XsensSuit::XsensSuitImpl::XsensVirtualSphericalJointKinSensor

bool getJointAccelerations(Vector3& accelerations) const override
{
if (m_suitImpl->virtualJointKinSensorsMap.find(this->m_name)
== m_suitImpl->virtualJointKinSensorsMap.end()) {
if (m_suitImpl->virtualSphericalJointKinSensorsMap.find(this->m_name)
== m_suitImpl->virtualSphericalJointKinSensorsMap.end()) {
yError() << logPrefix << "Sensor" << this->m_name << "NOT found";
accelerations.fill(0.0);
return false;
}

// Retrieve data sample directly form XSens Driver
const auto jointData = m_suitImpl->driver->getJointDataSample().data.at(
m_suitImpl->virtualJointKinSensorsMap.at(this->m_name).driverIndex);
m_suitImpl->virtualSphericalJointKinSensorsMap.at(this->m_name).driverIndex);

// TODO: This should be guaranteed, runtime check should be removed
// Check if suit sensor and driver data sample have the same name
Expand Down Expand Up @@ -626,8 +626,8 @@ void XsensSuit::XsensSuitImpl::setAllSensorStates(wearable::sensor::SensorStatus
for (const auto& vlks : virtualLinkKinSensorsMap) {
vlks.second.xsSensor->setStatus(aStatus);
}
for (const auto& vjks : virtualJointKinSensorsMap) {
vjks.second.xsSensor->setStatus(aStatus);
for (const auto& vsjks : virtualSphericalJointKinSensorsMap) {
vsjks.second.xsSensor->setStatus(aStatus);
}
}

Expand Down Expand Up @@ -785,7 +785,7 @@ bool XsensSuit::open(yarp::os::Searchable& config)
std::string posePrefix = getWearableName() + sensor::IPoseSensor::getPrefix();
std::string magPrefix = getWearableName() + sensor::IMagnetometer::getPrefix();
std::string vlksPrefix = getWearableName() + sensor::IVirtualLinkKinSensor::getPrefix();
std::string vjksPrefix =
std::string vsjksPrefix =
getWearableName() + sensor::IVirtualSphericalJointKinSensor::getPrefix();

if (pImpl->driver->getDriverConfiguration().dataStreamConfiguration.enableSensorData) {
Expand Down Expand Up @@ -852,10 +852,10 @@ bool XsensSuit::open(yarp::os::Searchable& config)
for (size_t s = 0; s < jointNames.size(); ++s) {
// Create the new sensor
auto sensor = std::make_shared<XsensSuitImpl::XsensVirtualSphericalJointKinSensor>(
pImpl.get(), vjksPrefix + jointNames[s]);
pImpl.get(), vsjksPrefix + jointNames[s]);
// Insert it in the output structure
pImpl->virtualJointKinSensorsMap.emplace(
vjksPrefix + jointNames[s],
pImpl->virtualSphericalJointKinSensorsMap.emplace(
vsjksPrefix + jointNames[s],
XsensSuitImpl::driverToDeviceSensors<
XsensSuitImpl::XsensVirtualSphericalJointKinSensor>{sensor, s});
}
Expand Down Expand Up @@ -1078,10 +1078,10 @@ XsensSuit::getSensors(const wearable::sensor::SensorType aType) const
break;
}
case sensor::SensorType::VirtualSphericalJointKinSensor: {
outVec.reserve(pImpl->virtualJointKinSensorsMap.size());
for (const auto& vjks : pImpl->virtualJointKinSensorsMap) {
outVec.reserve(pImpl->virtualSphericalJointKinSensorsMap.size());
for (const auto& vsjks : pImpl->virtualSphericalJointKinSensorsMap) {
outVec.push_back(
static_cast<std::shared_ptr<sensor::ISensor>>(vjks.second.xsSensor));
static_cast<std::shared_ptr<sensor::ISensor>>(vsjks.second.xsSensor));
}

break;
Expand Down Expand Up @@ -1189,13 +1189,13 @@ wearable::SensorPtr<const wearable::sensor::IVirtualSphericalJointKinSensor>
XsensSuit::getVirtualSphericalJointKinSensor(const wearable::sensor::SensorName name) const
{
// Check if user-provided name corresponds to an available sensor
if (pImpl->virtualJointKinSensorsMap.find(static_cast<std::string>(name))
== pImpl->virtualJointKinSensorsMap.end()) {
if (pImpl->virtualSphericalJointKinSensorsMap.find(static_cast<std::string>(name))
== pImpl->virtualSphericalJointKinSensorsMap.end()) {
yError() << logPrefix << "Invalid sensor name";
return nullptr;
}

// Return a shared pointer to the required sensor
return static_cast<std::shared_ptr<sensor::IVirtualSphericalJointKinSensor>>(
pImpl->virtualJointKinSensorsMap.at(static_cast<std::string>(name)).xsSensor);
pImpl->virtualSphericalJointKinSensorsMap.at(static_cast<std::string>(name)).xsSensor);
}

0 comments on commit 4f55e8c

Please sign in to comment.