Skip to content

Commit

Permalink
new transforminterfaces: added c++11 version
Browse files Browse the repository at this point in the history
  • Loading branch information
Andrea Ruzzenenti committed Jun 3, 2019
1 parent 21b13fe commit 4a9a045
Show file tree
Hide file tree
Showing 5 changed files with 66 additions and 12 deletions.
12 changes: 10 additions & 2 deletions src/devices/FrameBroadcaster/FrameBroadcaster.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,8 @@ using namespace yarp::rosmsg::tf2_msgs;
inline void BottleaddFrame(TFMessage& b, const yarp::math::FrameTransform& frame)
{

auto& submsg = yarp::rosmsg:: geometry_msgs::TransformStamped();
yarp::rosmsg::geometry_msgs::TransformStamped submsg;

submsg.child_frame_id = frame.frameId;
submsg.header.frame_id = frame.parentFrame;
submsg.transform.rotation.w = frame.rotation.w();
Expand Down Expand Up @@ -77,7 +78,11 @@ bool FrameBroadcaster::open(yarp::os::Searchable& params)
};

static int id = 0;
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
n = std::unique_ptr<yarp::os::Node>(new yarp::os::Node("/frameBroadcaster" + std::to_string(id)));
#else
n = std::make_unique<yarp::os::Node>("/frameBroadcaster" + std::to_string(id));
#endif
id++;

if(params.check("help"))
Expand Down Expand Up @@ -155,8 +160,11 @@ bool FrameBroadcaster::open(yarp::os::Searchable& params)
bool FrameBroadcaster::openAndAttachSubDevice(Searchable& prop)
{
Property p;

#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
m_subDeviceOwned = unique_ptr<PolyDriver>(new PolyDriver);
#else
m_subDeviceOwned = std::make_unique<PolyDriver>();
#endif

p.fromString(prop.toString());
p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
Expand Down
9 changes: 8 additions & 1 deletion src/devices/FrameReceiver/FrameReceiver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@ using namespace yarp::rosmsg::tf2_msgs;
void PortCallback::onRead(TFMessage& msg)
{
int i = 0;
Bottle* submsg{ nullptr };

FrameTransform ft;
owner->cacheValid = false;
Expand All @@ -36,7 +35,11 @@ void PortCallback::onRead(TFMessage& msg)
ft.rotation.x() = submsg.transform.rotation.x;
ft.rotation.y() = submsg.transform.rotation.y;
ft.rotation.z() = submsg.transform.rotation.z;
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
ft.timestamp = submsg.header.stamp.sec + submsg.header.stamp.nsec/1000000000;
#else
ft.timestamp = submsg.header.stamp.sec + submsg.header.stamp.nsec/1'000'000'000;
#endif
fvec[ft.frameId] = ft;
i++;
}
Expand Down Expand Up @@ -69,7 +72,11 @@ void FrameReceiver::updateFrameContainer(FrameEditor& fs)
bool FrameReceiver::open(yarp::os::Searchable& params)
{
static int id = 0;
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
n = std::unique_ptr<yarp::os::Node>(new yarp::os::Node("/frameReceiver" + std::to_string(id)));
#else
n = std::make_unique<yarp::os::Node>("/frameReceiver" + std::to_string(id));
#endif
id++;
if(params.check("help"))
{
Expand Down
19 changes: 15 additions & 4 deletions src/libYARP_dev/include/yarp/dev/IFrameSource.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,13 @@ class YARP_dev_API yarp::dev::IFrameSource
class Result
{
public:
bool valid{ false };
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
bool valid;
#else
bool valid{false};
#endif
int error;

T value;
//adding a operator bool(){return valid;} is a very bad idea as it can cause confusion between the result of bool functions and their success
};
Expand Down Expand Up @@ -182,10 +187,15 @@ class YARP_dev_API yarp::dev::ImplementIFrameSource : public yarp::dev::IFrameSo
yarp::math::FrameTransform nullframe;

public:
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
const std::map<std::string, yarp::math::FrameTransform>::const_iterator begin() const { return storage.begin(); }
const std::map<std::string, yarp::math::FrameTransform>::const_iterator end() const { return storage.end(); }
const size_t erase(const std::string& k) { return storage.erase(k); }
#else
const auto begin() const { return storage.begin(); }
const auto end() const { return storage.end(); }
const auto erase(const std::string& k) { return storage.erase(k); }
#endif
bool insertUpdate(const yarp::math::FrameTransform& frame)
{
storage[frame.frameId] = frame;
Expand Down Expand Up @@ -220,6 +230,7 @@ class YARP_dev_API yarp::dev::ImplementIFrameSource : public yarp::dev::IFrameSo
for (auto callback : callbacks)
if (callback.second)
callback.second((IFrameSource*)this);
return true;
}

/**
Expand Down Expand Up @@ -262,8 +273,8 @@ class YARP_dev_API yarp::dev::ImplementIFrameSource : public yarp::dev::IFrameSo
virtual bool unsetCallback(const int& id) override;
virtual Result<int> setCallback(std::function<bool(IFrameSource*)> cb) override;
virtual std::string allFramesAsString() override;
virtual bool clearOlderFrames(const std::chrono::milliseconds& lifeMax);
virtual bool clearStaticFrames();
virtual bool clearOlderFrames(const std::chrono::milliseconds& lifeMax) override;
virtual bool clearStaticFrames() override;
virtual Result<bool> canTransform(const std::string &target_frame, const std::string &source_frame) override;
virtual bool frameExists(const std::string &frame_id) override;
virtual std::unordered_set<std::string> getAllFrameIds() override;
Expand Down
14 changes: 10 additions & 4 deletions src/libYARP_dev/src/ImplementIFrameSource.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,8 @@

#include <yarp/dev/IFrameSource.h>
#include <algorithm>
#include<thread>
#include <iterator>
#include <thread>
using namespace yarp::dev;
using namespace yarp::math;
using namespace yarp::sig;
Expand Down Expand Up @@ -201,7 +202,11 @@ std::vector<FrameTransform> ImplementIFrameSource::getAllFrames()
if (!cacheValid)
updateFrameContainer(frameContainer);
std::vector<FrameTransform> v;
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
std::transform(frameContainer.begin(), frameContainer.end(), std::back_inserter(v), [](const pair<string, FrameTransform> &kv)->FrameTransform { return kv.second; });
#else
std::transform(frameContainer.begin(), frameContainer.end(), std::back_inserter(v), [](auto &kv) { return kv.second; });
#endif
return v;
}

Expand All @@ -215,7 +220,11 @@ IFrameSource::Result<std::string> ImplementIFrameSource::getParent(const std::st

IFrameSource::Result<std::string> ImplementIFrameSource::getParentRaw(const std::string& frame_id)
{
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
auto it = std::find_if(FULL_RANGE(frameContainer), [&frame_id](const pair<string, FrameTransform>& frame)->bool { return frame.second.frameId == frame_id; });
#else
auto it = std::find_if(FULL_RANGE(frameContainer), [&frame_id](const auto& frame) { return frame.second.frameId == frame_id; });
#endif
if (it == frameContainer.end())
{
return { false, IFrameSource::FRAME_NOT_FOUND, ""};
Expand All @@ -226,9 +235,6 @@ IFrameSource::Result<std::string> ImplementIFrameSource::getParentRaw(const std:

IFrameSource::Result<FrameTransform> ImplementIFrameSource::getChainedTransform(const std::string& target_frame_id, const std::string& source_frame_id)
{
const auto& tfVec = frameContainer;
size_t i;

for (const auto& i : frameContainer)
{
if (i.second.frameId == source_frame_id)
Expand Down
24 changes: 23 additions & 1 deletion tests/libYARP_dev/FrameTransformClientTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
#include <yarp/os/Network.h>
#include <yarp/os/Time.h>
#include <yarp/math/Math.h>

#include <chrono>
#include <cmath>
#include <vector>

Expand All @@ -27,7 +27,9 @@
using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::math;
#if (defined _WIN32 && _MSC_VER > 1910) || (!defined _WIN32 && __cplusplus > 201402L)
using namespace std::chrono_literals;
#endif

static bool isEqual(const yarp::sig::Vector& v1, const yarp::sig::Vector& v2, double precision)
{
Expand Down Expand Up @@ -282,15 +284,23 @@ TEST_CASE("dev::FrameTransformClientTest", "[yarp::dev]")
yarp::os::Time::delay(1);
bool del_bool = itfR->frameExists("frame_test");
yarp::os::Time::delay(1);
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
itfR->clearOlderFrames(std::chrono::milliseconds(10));
#else
itfR->clearOlderFrames(10ms);
#endif
del_bool &= (!itfR->frameExists("frame_test"));
CHECK(del_bool); // deleteTransform ok
}

//test 9
{
itfB->clear();
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
itfR->clearOlderFrames(std::chrono::milliseconds(0));
#else
itfR->clearOlderFrames(0ms);
#endif
itfR->clearStaticFrames();
auto cids = itfR->getAllFrameIds();
CHECK(cids.size() == 0); // clear ok
Expand All @@ -301,14 +311,22 @@ TEST_CASE("dev::FrameTransformClientTest", "[yarp::dev]")
itfB->setTransform({"frame10", "frame2", m1});
yarp::os::Time::delay(0.050);
CHECK(itfR->canTransform("frame10", "frame2").value); // itf->setTransform ok
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
itfR->clearOlderFrames(std::chrono::milliseconds(40));
#else
itfR->clearOlderFrames(40ms);
#endif
CHECK_FALSE(itfR->canTransform("frame10", "frame2").value); // itf->setTransform successfully expired after 0.6s
}

//test 11
{
itfB->clear();
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
itfR->clearOlderFrames(std::chrono::milliseconds(0));
#else
itfR->clearOlderFrames(0ms);
#endif
itfR->clearStaticFrames();
bool set_b1 = itfB->setTransform({ "frame10", "frame2", m1 });
yarp::os::Time::delay(0.050);
Expand Down Expand Up @@ -348,7 +366,11 @@ TEST_CASE("dev::FrameTransformClientTest", "[yarp::dev]")
//test 12
{
itfB->clear();
#if (defined _WIN32 && _MSC_VER < 1910) || (!defined _WIN32 && __cplusplus < 201402L)
itfR->clearOlderFrames(std::chrono::milliseconds(0));
#else
itfR->clearOlderFrames(0ms);
#endif
itfR->clearStaticFrames();
CHECK(itfB->setTransform({ "frame1", "frame2", m1 }));
yarp::os::Time::delay(0.050);
Expand Down

0 comments on commit 4a9a045

Please sign in to comment.