diff --git a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp index 472da5d901..52955aae71 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include namespace uavcan_linux @@ -54,14 +55,22 @@ class Node : public uavcan::Node DriverPackPtr driver_pack_; DefaultLogSink log_sink_; - static void enforce(int error, const char* msg) + static void enforce(int error, const std::string& msg) { if (error < 0) { - throw Exception(msg); + std::ostringstream os; + os << msg << " [" << error << "]"; + throw Exception(os.str()); } } + template + static std::string getDataTypeName() + { + return DataType::getDataTypeFullName(); + } + public: /** * Simple forwarding constructor, compatible with uavcan::Node @@ -87,7 +96,7 @@ public: makeSubscriber(const typename uavcan::Subscriber::Callback& cb) { std::shared_ptr> p(new uavcan::Subscriber(*this)); - enforce(p->start(cb), "Subscriber start"); + enforce(p->start(cb), "Subscriber start failure " + getDataTypeName()); return p; } @@ -96,7 +105,7 @@ public: makePublisher(uavcan::MonotonicDuration tx_timeout = uavcan::Publisher::getDefaultTxTimeout()) { std::shared_ptr> p(new uavcan::Publisher(*this)); - enforce(p->init(), "Publisher init"); + enforce(p->init(), "Publisher init failure " + getDataTypeName()); p->setTxTimeout(tx_timeout); return p; } @@ -106,7 +115,7 @@ public: makeServiceServer(const typename uavcan::ServiceServer::Callback& cb) { std::shared_ptr> p(new uavcan::ServiceServer(*this)); - enforce(p->start(cb), "ServiceServer start"); + enforce(p->start(cb), "ServiceServer start failure " + getDataTypeName()); return p; } @@ -115,7 +124,7 @@ public: makeServiceClient(const typename uavcan::ServiceClient::Callback& cb) { std::shared_ptr> p(new uavcan::ServiceClient(*this)); - enforce(p->init(), "ServiceClient init"); + enforce(p->init(), "ServiceClient init failure " + getDataTypeName()); p->setCallback(cb); return p; }