Skip to content

Commit

Permalink
[devices][FloatingBaseEstimatorDevice] add TF broadcaster
Browse files Browse the repository at this point in the history
  • Loading branch information
@prashanthr05
prashanthr05 committed Nov 10, 2020
1 parent c41ecec commit 4e1a6f6
Show file tree
Hide file tree
Showing 2 changed files with 80 additions and 18 deletions.
6 changes: 6 additions & 0 deletions devices/FloatingBaseEstimatorDevice/include/BipedalLocomotion/FloatingBaseEstimatorDevice.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/Wrapper.h>
#include <yarp/os/ResourceFinder.h>
#include <yarp/dev/IFrameTransform.h>

#include <mutex>
#include <memory>
Expand Down Expand Up @@ -49,6 +50,7 @@ class BipedalLocomotion::FloatingBaseEstimatorDevice : public yarp::dev::DeviceD
bool setupFeetContactStateMachines(yarp::os::Searchable& config);
bool parseFootSchmittParams(yarp::os::Searchable& config, iDynTree::SchmittParams& params);
bool setupBaseEstimator(yarp::os::Searchable& config);
bool loadTransformBroadcaster();
bool updateMeasurements();
bool updateInertialBuffers();
bool updateContactStates();
Expand Down Expand Up @@ -85,6 +87,10 @@ class BipedalLocomotion::FloatingBaseEstimatorDevice : public yarp::dev::DeviceD
std::string m_baseLinkImuName{"root_link_imu_acc"};
std::string m_leftFootWrenchName{"left_foot_cartesian_wrench"};
std::string m_rightFootWrenchName{"right_foot_cartesian_wrench"};

yarp::dev::PolyDriver m_transformBroadcaster;
yarp::dev::IFrameTransform* m_transformInterface{nullptr};
bool m_publishROSTF{false};
};


Expand Down
92 changes: 74 additions & 18 deletions devices/FloatingBaseEstimatorDevice/src/FloatingBaseEstimatorDevice.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@

#include <BipedalLocomotion/FloatingBaseEstimators/InvariantEKFBaseEstimator.h>
#include <iDynTree/Core/EigenHelpers.h>
#include <iDynTree/yarp/YARPConversions.h>
#include <yarp/os/LogStream.h>

using namespace BipedalLocomotion;
Expand All @@ -23,7 +24,6 @@ FloatingBaseEstimatorDevice::FloatingBaseEstimatorDevice(double period,
{
}


FloatingBaseEstimatorDevice::FloatingBaseEstimatorDevice()
: yarp::os::PeriodicThread(0.01, yarp::os::ShouldUseSystemClock::No)
{
Expand Down Expand Up @@ -67,10 +67,23 @@ bool FloatingBaseEstimatorDevice::open(yarp::os::Searchable& config)
return false;
}

if (!YarpUtilities::getElementFromSearchable(config, "publish_rostf", m_publishROSTF))
{
m_publishROSTF = false;
}

if (m_publishROSTF)
{
if (!loadTransformBroadcaster())
{
return false;
}
}

return true;
}

bool BipedalLocomotion::FloatingBaseEstimatorDevice::setupRobotModel(yarp::os::Searchable& config)
bool FloatingBaseEstimatorDevice::setupRobotModel(yarp::os::Searchable& config)
{

std::string modelFileName;
Expand Down Expand Up @@ -102,7 +115,7 @@ bool BipedalLocomotion::FloatingBaseEstimatorDevice::setupRobotModel(yarp::os::S
return true;
}

bool BipedalLocomotion::FloatingBaseEstimatorDevice::setupRobotSensorBridge(yarp::os::Searchable& config)
bool FloatingBaseEstimatorDevice::setupRobotSensorBridge(yarp::os::Searchable& config)
{
auto bridgeConfig = config.findGroup("RobotSensorBridge");
if (bridgeConfig.isNull())
Expand All @@ -124,7 +137,7 @@ bool BipedalLocomotion::FloatingBaseEstimatorDevice::setupRobotSensorBridge(yarp
return true;
}

bool BipedalLocomotion::FloatingBaseEstimatorDevice::setupFeetContactStateMachines(yarp::os::Searchable& config)
bool FloatingBaseEstimatorDevice::setupFeetContactStateMachines(yarp::os::Searchable& config)
{
auto csmConfig = config.findGroup("ContactSchmittTrigger");
if (csmConfig.isNull())
Expand Down Expand Up @@ -164,8 +177,8 @@ bool BipedalLocomotion::FloatingBaseEstimatorDevice::setupFeetContactStateMachin
return true;
}

bool BipedalLocomotion::FloatingBaseEstimatorDevice::parseFootSchmittParams(yarp::os::Searchable& config,
iDynTree::SchmittParams& params)
bool FloatingBaseEstimatorDevice::parseFootSchmittParams(yarp::os::Searchable& config,
iDynTree::SchmittParams& params)
{
if (!YarpUtilities::getElementFromSearchable(config, "schmitt_stable_contact_make_time", params.stableTimeContactMake)) { return false; }
if (!YarpUtilities::getElementFromSearchable(config, "schmitt_stable_contact_break_time", params.stableTimeContactBreak)) { return false; }
Expand All @@ -175,7 +188,7 @@ bool BipedalLocomotion::FloatingBaseEstimatorDevice::parseFootSchmittParams(yarp
return true;
}

bool BipedalLocomotion::FloatingBaseEstimatorDevice::setupBaseEstimator(yarp::os::Searchable& config)
bool FloatingBaseEstimatorDevice::setupBaseEstimator(yarp::os::Searchable& config)
{
if (!YarpUtilities::getElementFromSearchable(config, "estimator_type", m_estimatorType))
{
Expand Down Expand Up @@ -218,7 +231,7 @@ bool FloatingBaseEstimatorDevice::attachAll(const yarp::dev::PolyDriverList & po
return true;
}

bool BipedalLocomotion::FloatingBaseEstimatorDevice::openCommunications()
bool FloatingBaseEstimatorDevice::openCommunications()
{
if (!openBufferedSigPort(m_comms.floatingBaseStatePort, m_portPrefix, "/floating_base/state:o"))
{
Expand All @@ -230,9 +243,9 @@ bool BipedalLocomotion::FloatingBaseEstimatorDevice::openCommunications()
return true;
}

bool BipedalLocomotion::FloatingBaseEstimatorDevice::openBufferedSigPort(yarp::os::BufferedPort<yarp::sig::Vector>& port,
const std::string& portPrefix,
const std::string& address)
bool FloatingBaseEstimatorDevice::openBufferedSigPort(yarp::os::BufferedPort<yarp::sig::Vector>& port,
const std::string& portPrefix,
const std::string& address)
{
bool ok{false};
ok = port.open(portPrefix + address);
Expand All @@ -246,12 +259,21 @@ bool BipedalLocomotion::FloatingBaseEstimatorDevice::openBufferedSigPort(yarp::o

void FloatingBaseEstimatorDevice::run()
{
// advance sensor bridge
if (!m_robotSensorBridge->advance())
{
std::cout << "Advance Sensor bridge failed." << std::endl;
return;
}

// update estimator measurements
if (!updateMeasurements())
{
std::cout << "Measurement updates failed." << std::endl;
return;
}

// advance estimator
if (!m_estimator->advance())
{
std::cout << "Advance estimator failed." << std::endl;
Expand All @@ -275,7 +297,6 @@ bool FloatingBaseEstimatorDevice::updateMeasurements()
bool FloatingBaseEstimatorDevice::updateInertialBuffers()
{
Eigen::Matrix<double, 12, 1> imuMeasure;
double timeStamp;
if (!m_robotSensorBridge->getIMUMeasurement(m_baseLinkImuName, imuMeasure))
{
return false;
Expand All @@ -296,18 +317,18 @@ bool FloatingBaseEstimatorDevice::updateContactStates()
Eigen::Matrix<double, 6, 1> lfWrench, rfWrench;
double lfTimeStamp, rfTimeStamp;
bool ok{true};
ok = ok && m_robotSensorBridge->getCartesianWrench(m_leftFootWrenchName, lfWrench);
ok = ok && m_robotSensorBridge->getCartesianWrench(m_leftFootWrenchName, lfWrench, &lfTimeStamp);
if (ok)
{
m_currentlContactNormal = lfWrench(2);
m_lFootCSM->contactMeasurementUpdate(yarp::os::Time::now(), lfWrench(2)); // fz
m_lFootCSM->contactMeasurementUpdate(lfTimeStamp, lfWrench(2)); // fz
}

ok = ok && m_robotSensorBridge->getCartesianWrench(m_rightFootWrenchName, rfWrench);
ok = ok && m_robotSensorBridge->getCartesianWrench(m_rightFootWrenchName, rfWrench, &rfTimeStamp);
if (ok)
{
m_currentrContactNormal = rfWrench(2);
m_rFootCSM->contactMeasurementUpdate(yarp::os::Time::now(), rfWrench(2)); // fz
m_rFootCSM->contactMeasurementUpdate(rfTimeStamp, rfWrench(2)); // fz
}

if (!ok)
Expand Down Expand Up @@ -379,6 +400,16 @@ void FloatingBaseEstimatorDevice::publishBaseLinkState(const FloatingBaseEstimat
for (size_t idx = angVelOffset; idx < stateVecSize; idx++) { stateVec(idx) = baseAngularVel(idx - angVelOffset); }

m_comms.floatingBaseStatePort.write();

yarp::sig::Matrix basePoseYARP;
iDynTree::toYarp(estimatorOut.basePose, basePoseYARP);
if (m_publishROSTF && m_transformInterface != nullptr)
{
if (!m_transformInterface->setTransform("/world", "/base_link", basePoseYARP))
{
yError() << "[FloatingBaseEstimatorDevice] Could not publish measured base pose transform from primary IMU";
}
}
}

void FloatingBaseEstimatorDevice::publishInternalStateAndStdDev(const FloatingBaseEstimators::Output& estimatorOut)
Expand Down Expand Up @@ -439,15 +470,20 @@ bool FloatingBaseEstimatorDevice::detachAll()
return true;
}

void BipedalLocomotion::FloatingBaseEstimatorDevice::closeBufferedSigPort(yarp::os::BufferedPort<yarp::sig::Vector>& port)
void FloatingBaseEstimatorDevice::closeBufferedSigPort(yarp::os::BufferedPort<yarp::sig::Vector>& port)
{
if (!port.isClosed())
{
port.close();
}

if (m_publishROSTF)
{
m_transformInterface = nullptr;
}
}

void BipedalLocomotion::FloatingBaseEstimatorDevice::closeCommunications()
void FloatingBaseEstimatorDevice::closeCommunications()
{
closeBufferedSigPort(m_comms.floatingBaseStatePort);
closeBufferedSigPort(m_comms.internalStateAndStdDevPort);
Expand All @@ -461,3 +497,23 @@ bool FloatingBaseEstimatorDevice::close()
return true;
}

bool FloatingBaseEstimatorDevice::loadTransformBroadcaster()
{
yarp::os::Property tfBroadcasterSettings{{"device", yarp::os::Value("transformClient")},
{"remote", yarp::os::Value("/transformServer")},
{"local", yarp::os::Value(m_portPrefix + "/transformClient")}};

if (!m_transformBroadcaster.open(tfBroadcasterSettings))
{
yError() << "[FloatingBaseEstimatorDevice][loadTransformBroadcaster] could not open transform broadcaster.";
return false;
}

if (!m_transformBroadcaster.view(m_transformInterface))
{
yError() << "[FloatingBaseEstimatorDevice][loadTransformBroadcaster] could not access transform interface";
return false;
}

return true;
}

0 comments on commit 4e1a6f6

Please sign in to comment.