diff --git a/src/devices/frameTransformClient/FrameTransformClient.cpp b/src/devices/frameTransformClient/FrameTransformClient.cpp index ba667793f8..f0dee1eaee 100644 --- a/src/devices/frameTransformClient/FrameTransformClient.cpp +++ b/src/devices/frameTransformClient/FrameTransformClient.cpp @@ -120,15 +120,20 @@ bool FrameTransformClient::read(yarp::os::ConnectionReader& connection) std::string full_port_name = m_local_name + "/" + port_name; //print a warning if the frames do not exists yet - if (this->frameExists(src) == false) + bool exists = false; + auto r1 = this->frameExists(src, exists); + if (!r1) { return r1; } + if (exists == false) { out.addString("Requested src frame " + src + " does not exists."); yCWarning(FRAMETRANSFORMCLIENT, "Requested src frame %s does not exists.", src.c_str()); } - if (this->frameExists(dst) == false) + auto r2 = this->frameExists(dst, exists); + if (!r2) { return r2; } + if (exists == false) { out.addString("Requested dst frame " + dst + " does not exists."); - yCWarning(FRAMETRANSFORMCLIENT, "Requested fst frame %s does not exists.", dst.c_str()); + yCWarning(FRAMETRANSFORMCLIENT, "Requested dst frame %s does not exists.", dst.c_str()); } //check if the port is already active in yarp @@ -427,16 +432,16 @@ bool FrameTransformClient::close() return true; } -bool FrameTransformClient::allFramesAsString(std::string &all_frames) +yarp::dev::ReturnValue FrameTransformClient::allFramesAsString(std::string &all_frames) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured", __func__); - return false; + return ReturnValue::return_code::return_value_error_generic; } FrameTransformContainer* p_cont = nullptr; - bool br = m_ift_util->getInternalContainer(p_cont); - if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; } + auto br = m_ift_util->getInternalContainer(p_cont); + if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return ReturnValue::return_code::return_value_error_generic; } //protect the internal container from concurrent access std::lock_guard l(p_cont->m_trf_mutex); @@ -446,7 +451,7 @@ bool FrameTransformClient::allFramesAsString(std::string &all_frames) { all_frames += it->toString() + " "; } - return true; + return ReturnValue_ok; } FrameTransformClient::ConnectionType FrameTransformClient::priv_getConnectionType(const std::string &target_frame, const std::string &source_frame, std::string* commonAncestor = nullptr) @@ -509,41 +514,50 @@ FrameTransformClient::ConnectionType FrameTransformClient::priv_getConnectionTyp return ConnectionType::DISCONNECTED; } -bool FrameTransformClient::canTransform(const std::string &target_frame, const std::string &source_frame) +yarp::dev::ReturnValue FrameTransformClient::canTransform(const std::string &target_frame, const std::string &source_frame, bool& canTransform) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured", __func__); - return false; + return ReturnValue::return_code::return_value_error_generic; + } + auto conntype = priv_getConnectionType(target_frame, source_frame); + if (conntype != ConnectionType::DISCONNECTED) + { + canTransform = true; } - return priv_getConnectionType(target_frame, source_frame) != ConnectionType::DISCONNECTED; + else + { + canTransform = false; + } + return ReturnValue_ok; } -bool FrameTransformClient::clear() +yarp::dev::ReturnValue FrameTransformClient::clear() { if(!m_ift_set) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageSet interface found. Your device is wrongly configured",__func__); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } if (m_ift_set) { return m_ift_set->clearAll(); } yCError(FRAMETRANSFORMCLIENT, "clear(): interface not available"); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } -bool FrameTransformClient::frameExists(const std::string &frame_id) +yarp::dev::ReturnValue FrameTransformClient::frameExists(const std::string &frame_id, bool& exists) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured", __func__); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } FrameTransformContainer* p_cont = nullptr; - bool br = m_ift_util->getInternalContainer(p_cont); - if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; } + auto br = m_ift_util->getInternalContainer(p_cont); + if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return ReturnValue::return_code::return_value_error_generic; } //protect the internal container from concurrent access std::lock_guard l(p_cont->m_trf_mutex); @@ -551,24 +565,25 @@ bool FrameTransformClient::frameExists(const std::string &frame_id) //process data for (auto it = p_cont->begin(); it != p_cont->end(); it++) { - if (it->src_frame_id == frame_id) { return true; } - if (it->dst_frame_id == frame_id) { return true; } + if (it->src_frame_id == frame_id) { exists = true; return ReturnValue_ok; } + if (it->dst_frame_id == frame_id) { exists = true; return ReturnValue_ok; } } // condition reached if not found or container is empty - return false; + exists = false; + return ReturnValue_ok; } -bool FrameTransformClient::getAllFrameIds(std::vector &ids) +yarp::dev::ReturnValue FrameTransformClient::getAllFrameIds(std::vector &ids) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } FrameTransformContainer* p_cont = nullptr; - bool br = m_ift_util->getInternalContainer(p_cont); - if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; } + auto br = m_ift_util->getInternalContainer(p_cont); + if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return ReturnValue::return_code::return_value_error_generic; } //protect the internal container from concurrent access std::lock_guard l(p_cont->m_trf_mutex); @@ -598,19 +613,19 @@ bool FrameTransformClient::getAllFrameIds(std::vector &ids) } } - return true; + return ReturnValue_ok; } -bool FrameTransformClient::getParent(const std::string &frame_id, std::string &parent_frame_id) +yarp::dev::ReturnValue FrameTransformClient::getParent(const std::string &frame_id, std::string &parent_frame_id) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } FrameTransformContainer* p_cont = nullptr; - bool br = m_ift_util->getInternalContainer(p_cont); - if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; } + auto br = m_ift_util->getInternalContainer(p_cont); + if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return ReturnValue::return_code::return_value_error_generic; } //protect the internal container from concurrent access std::lock_guard l(p_cont->m_trf_mutex); @@ -622,10 +637,10 @@ bool FrameTransformClient::getParent(const std::string &frame_id, std::string &p if (it->dst_frame_id == frame_id) { parent_frame_id = it->src_frame_id; - return true; + return ReturnValue_ok; } } - return false; + return ReturnValue::return_code::return_value_error_method_failed; } bool FrameTransformClient::priv_canExplicitTransform(const std::string& target_frame_id, const std::string& source_frame_id) const @@ -681,26 +696,34 @@ bool FrameTransformClient::priv_getChainedTransform(const std::string& target_fr return false; } -bool FrameTransformClient::getTransform(const std::string& target_frame_id, const std::string& source_frame_id, yarp::sig::Matrix& transform) +yarp::dev::ReturnValue FrameTransformClient::getTransform(const std::string& target_frame_id, const std::string& source_frame_id, yarp::sig::Matrix& transform) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } ConnectionType ct; std::string ancestor; ct = priv_getConnectionType(target_frame_id, source_frame_id, &ancestor); if (ct == ConnectionType::DIRECT) { - return priv_getChainedTransform(target_frame_id, source_frame_id, transform); + bool ok = priv_getChainedTransform(target_frame_id, source_frame_id, transform); + if (ok) + { + return ReturnValue_ok; + } + else + { + return ReturnValue::return_code::return_value_error_method_failed; + } } else if (ct == ConnectionType::INVERSE) { yarp::sig::Matrix m(4, 4); priv_getChainedTransform(source_frame_id, target_frame_id, m); transform = yarp::math::SE3inv(m); - return true; + return ReturnValue_ok; } else if(ct == ConnectionType::UNDIRECT) { @@ -708,37 +731,41 @@ bool FrameTransformClient::getTransform(const std::string& target_frame_id, cons priv_getChainedTransform(source_frame_id, ancestor, root2src); priv_getChainedTransform(target_frame_id, ancestor, root2tar); transform = yarp::math::SE3inv(root2src) * root2tar; - return true; + return ReturnValue_ok; } else if (ct == ConnectionType::IDENTITY) { yarp::sig::Matrix tmp(4, 4); tmp.eye(); transform = tmp; - return true; + return ReturnValue_ok; } yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "getTransform(): Frames " << source_frame_id << " and " << target_frame_id << " are not connected"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } -bool FrameTransformClient::setTransform(const std::string& target_frame_id, const std::string& source_frame_id, const yarp::sig::Matrix& transform) +yarp::dev::ReturnValue FrameTransformClient::setTransform(const std::string& target_frame_id, const std::string& source_frame_id, const yarp::sig::Matrix& transform) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } if(target_frame_id == source_frame_id) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransform(): Invalid transform detected.\n" \ "\t Source frame and target frame are both equal to " << source_frame_id; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } - if (!priv_canExplicitTransform(target_frame_id, source_frame_id) && canTransform(target_frame_id, source_frame_id)) + bool b1 = priv_canExplicitTransform(target_frame_id, source_frame_id); + bool bcanTransform = false; + auto retc = canTransform(target_frame_id, source_frame_id, bcanTransform); + if (!retc) { return retc; } + if (!b1 && bcanTransform) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransform(): Such transform already exist by chaining transforms"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } yarp::os::Bottle b; @@ -748,7 +775,7 @@ bool FrameTransformClient::setTransform(const std::string& target_frame_id, cons if (!tf.fromMatrix(transform)) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransform(): Wrong matrix format, it has to be 4 by 4"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } tf.src_frame_id = source_frame_id; @@ -761,34 +788,36 @@ bool FrameTransformClient::setTransform(const std::string& target_frame_id, cons return m_ift_set->setTransform(tf); } yCError(FRAMETRANSFORMCLIENT, "setTransform(): interface not available"); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } -bool FrameTransformClient::setTransformStatic(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) +yarp::dev::ReturnValue FrameTransformClient::setTransformStatic(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } if(target_frame_id == source_frame_id) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransformStatic(): Invalid transform detected.\n" \ "\t Source frame and target frame are both equal to " << source_frame_id; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } - if (canTransform(target_frame_id, source_frame_id)) + bool bcanTransform = false; + canTransform(target_frame_id, source_frame_id, bcanTransform); + if (bcanTransform) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransform(): Such static transform already exist, directly or by chaining transforms"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } FrameTransform tf; if (!tf.fromMatrix(transform)) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransform(): Wrong matrix format, it has to be 4 by 4"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } tf.src_frame_id = source_frame_id; tf.dst_frame_id = target_frame_id; @@ -800,55 +829,55 @@ bool FrameTransformClient::setTransformStatic(const std::string &target_frame_id return m_ift_set->setTransform(tf); } yCError(FRAMETRANSFORMCLIENT, "setTransformStatic(): interface not available"); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } -bool FrameTransformClient::deleteTransform(const std::string &target_frame_id, const std::string &source_frame_id) +yarp::dev::ReturnValue FrameTransformClient::deleteTransform(const std::string &target_frame_id, const std::string &source_frame_id) { if (m_ift_set) { return m_ift_set->deleteTransform(target_frame_id, source_frame_id); } yCError(FRAMETRANSFORMCLIENT, "deleteTransform(): interface not available"); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } -bool FrameTransformClient::transformPoint(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_point, yarp::sig::Vector &transformed_point) +yarp::dev::ReturnValue FrameTransformClient::transformPoint(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_point, yarp::sig::Vector &transformed_point) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } if (input_point.size() != 3) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "Only 3 dimensional vector allowed."; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } yarp::sig::Matrix m(4, 4); if (!getTransform(target_frame_id, source_frame_id, m)) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "No transform found between source '" << target_frame_id << "' and target '" << source_frame_id << "'"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } yarp::sig::Vector in = input_point; in.push_back(1); transformed_point = m * in; transformed_point.pop_back(); - return true; + return ReturnValue_ok; } -bool FrameTransformClient::transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose) +yarp::dev::ReturnValue FrameTransformClient::transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } if (input_pose.size() != 6) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "Only 6 dimensional vector (3 axes + roll pith and yaw) allowed."; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } if (transformed_pose.size() != 6) { @@ -859,7 +888,7 @@ bool FrameTransformClient::transformPose(const std::string &target_frame_id, con if (!getTransform(target_frame_id, source_frame_id, m)) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "No transform found between source '" << target_frame_id << "' and target '" << source_frame_id << "'"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } FrameTransform t; t.transFromVec(input_pose[0], input_pose[1], input_pose[2]); @@ -874,47 +903,48 @@ bool FrameTransformClient::transformPose(const std::string &target_frame_id, con transformed_pose[3] = rot[0]; transformed_pose[4] = rot[1]; transformed_pose[5] = rot[2]; - return true; + return ReturnValue_ok; } -bool FrameTransformClient::transformQuaternion(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::math::Quaternion &input_quaternion, yarp::math::Quaternion &transformed_quaternion) +yarp::dev::ReturnValue FrameTransformClient::transformQuaternion(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::math::Quaternion &input_quaternion, yarp::math::Quaternion &transformed_quaternion) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } yarp::sig::Matrix m(4, 4); if (!getTransform(target_frame_id, source_frame_id, m)) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "No transform found between source '" << target_frame_id << "' and target '" << source_frame_id <<"'"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } FrameTransform t; t.rotation=input_quaternion; transformed_quaternion.fromRotationMatrix(m * t.toMatrix()); - return true; + return ReturnValue_ok; } -bool FrameTransformClient::waitForTransform(const std::string &target_frame_id, const std::string &source_frame_id, const double &timeout) +yarp::dev::ReturnValue FrameTransformClient::waitForTransform(const std::string &target_frame_id, const std::string &source_frame_id, const double &timeout) { if(!m_ift_util) { yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); - return false; + return ReturnValue::return_code::return_value_error_not_ready; } //loop until canTransform == true or timeout expires double start = yarp::os::SystemClock::nowSystem(); - while (!canTransform(target_frame_id, source_frame_id)) - { - if (yarp::os::SystemClock::nowSystem() - start > timeout) - { + bool bcan = false; + while (!bcan) { + if (yarp::os::SystemClock::nowSystem() - start > timeout) { yCError(FRAMETRANSFORMCLIENT) << "waitForTransform(): timeout expired"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } yarp::os::SystemClock::delaySystem(0.001); + ReturnValue ret = canTransform(target_frame_id, source_frame_id, bcan); + if (!ret) { return ret;} } - return true; + return ReturnValue_ok; } FrameTransformClient::FrameTransformClient() : PeriodicThread(0.01) diff --git a/src/devices/frameTransformClient/FrameTransformClient.h b/src/devices/frameTransformClient/FrameTransformClient.h index 7a8ffb7d26..34c629731a 100644 --- a/src/devices/frameTransformClient/FrameTransformClient.h +++ b/src/devices/frameTransformClient/FrameTransformClient.h @@ -106,20 +106,20 @@ class FrameTransformClient : bool read(yarp::os::ConnectionReader& connection) override; //IFrameTransform - bool allFramesAsString(std::string &all_frames) override; - bool canTransform(const std::string &target_frame, const std::string &source_frame) override; - bool clear() override; - bool frameExists(const std::string &frame_id) override; - bool getAllFrameIds(std::vector< std::string > &ids) override; - bool getParent(const std::string &frame_id, std::string &parent_frame_id) override; - bool getTransform(const std::string &target_frame_id, const std::string &source_frame_id, yarp::sig::Matrix &transform) override; - bool setTransform(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) override; - bool setTransformStatic(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) override; - bool deleteTransform(const std::string &target_frame_id, const std::string &source_frame_id) override; - bool transformPoint(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_point, yarp::sig::Vector &transformed_point) override; - bool transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose) override; - bool transformQuaternion(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::math::Quaternion &input_quaternion, yarp::math::Quaternion &transformed_quaternion) override; - bool waitForTransform(const std::string &target_frame_id, const std::string &source_frame_id, const double &timeout) override; + yarp::dev::ReturnValue allFramesAsString(std::string &all_frames) override; + yarp::dev::ReturnValue canTransform(const std::string &target_frame, const std::string &source_frame, bool& exists) override; + yarp::dev::ReturnValue clear() override; + yarp::dev::ReturnValue frameExists(const std::string &frame_id, bool& exists) override; + yarp::dev::ReturnValue getAllFrameIds(std::vector< std::string > &ids) override; + yarp::dev::ReturnValue getParent(const std::string &frame_id, std::string &parent_frame_id) override; + yarp::dev::ReturnValue getTransform(const std::string &target_frame_id, const std::string &source_frame_id, yarp::sig::Matrix &transform) override; + yarp::dev::ReturnValue setTransform(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) override; + yarp::dev::ReturnValue setTransformStatic(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) override; + yarp::dev::ReturnValue deleteTransform(const std::string &target_frame_id, const std::string &source_frame_id) override; + yarp::dev::ReturnValue transformPoint(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_point, yarp::sig::Vector &transformed_point) override; + yarp::dev::ReturnValue transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose) override; + yarp::dev::ReturnValue transformQuaternion(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::math::Quaternion &input_quaternion, yarp::math::Quaternion &transformed_quaternion) override; + yarp::dev::ReturnValue waitForTransform(const std::string &target_frame_id, const std::string &source_frame_id, const double &timeout) override; //PeriodicThread bool threadInit() override; diff --git a/src/devices/frameTransformGet/FrameTransformGetMultiplexer.cpp b/src/devices/frameTransformGet/FrameTransformGetMultiplexer.cpp index b248db95c3..a10d2b56d3 100644 --- a/src/devices/frameTransformGet/FrameTransformGetMultiplexer.cpp +++ b/src/devices/frameTransformGet/FrameTransformGetMultiplexer.cpp @@ -75,7 +75,7 @@ bool FrameTransformGetMultiplexer::attachAll(const yarp::dev::PolyDriverList& de } -bool FrameTransformGetMultiplexer::getTransforms(std::vector& transforms) const +yarp::dev::ReturnValue FrameTransformGetMultiplexer::getTransforms(std::vector& transforms) const { for (size_t i = 0; i < m_iFrameTransformStorageGetList.size(); i++) { @@ -86,10 +86,11 @@ bool FrameTransformGetMultiplexer::getTransforms(std::vector& transforms) const override; + yarp::dev::ReturnValue getTransforms(std::vector& transforms) const override; private: int m_verbose{4}; diff --git a/src/devices/frameTransformGet/FrameTransformGet_nwc_yarp.cpp b/src/devices/frameTransformGet/FrameTransformGet_nwc_yarp.cpp index 4681164ba3..1442c9d5ee 100644 --- a/src/devices/frameTransformGet/FrameTransformGet_nwc_yarp.cpp +++ b/src/devices/frameTransformGet/FrameTransformGet_nwc_yarp.cpp @@ -8,6 +8,8 @@ #include #include +using namespace yarp::dev; + YARP_LOG_COMPONENT(FRAMETRANSFORMGETNWCYARP, "yarp.devices.FrameTransformGet_nwc_yarp") @@ -136,7 +138,7 @@ bool FrameTransformGet_nwc_yarp::close() } -bool FrameTransformGet_nwc_yarp::getTransforms(std::vector& transforms) const +ReturnValue FrameTransformGet_nwc_yarp::getTransforms(std::vector& transforms) const { if (!m_streaming_port_enabled) { @@ -144,10 +146,10 @@ bool FrameTransformGet_nwc_yarp::getTransforms(std::vectorgetData(retrievedFromSteaming); transforms = retrievedFromSteaming.transforms_list; - return true; + return retrievedFromSteaming.retvalue; } yCError(FRAMETRANSFORMGETNWCYARP, "Unable to get transformations"); - return false; + return yarp::dev::ReturnValue::return_code::return_value_error_generic; } } diff --git a/src/devices/frameTransformGet/FrameTransformGet_nwc_yarp.h b/src/devices/frameTransformGet/FrameTransformGet_nwc_yarp.h index dd29762ab6..6d6f2b2dda 100644 --- a/src/devices/frameTransformGet/FrameTransformGet_nwc_yarp.h +++ b/src/devices/frameTransformGet/FrameTransformGet_nwc_yarp.h @@ -126,7 +126,7 @@ class FrameTransformGet_nwc_yarp: bool close() override; // yarp::dev::IFrameTransformStorageGet - bool getTransforms(std::vector& transforms) const override; + yarp::dev::ReturnValue getTransforms(std::vector& transforms) const override; private: int m_verbose{4}; diff --git a/src/devices/frameTransformGet/FrameTransformGet_nws_yarp.cpp b/src/devices/frameTransformGet/FrameTransformGet_nws_yarp.cpp index a198df97f7..82f9bafd39 100644 --- a/src/devices/frameTransformGet/FrameTransformGet_nws_yarp.cpp +++ b/src/devices/frameTransformGet/FrameTransformGet_nws_yarp.cpp @@ -7,7 +7,9 @@ #include #include +#include +using namespace yarp::dev; YARP_LOG_COMPONENT(FRAMETRANSFORMGETNWSYARP, "yarp.devices.FrameTransformGet_nws_yarp") @@ -143,26 +145,37 @@ bool FrameTransformGet_nws_yarp::attach( yarp::dev::PolyDriver* deviceToAttach) return_getAllTransforms FrameTransformGet_nws_yarp::getTransformsRPC() { std::lock_guard m_lock(m_mutex); - if (m_iFrameTransformStorageGet != nullptr) { + + return_getAllTransforms ret; + if (m_iFrameTransformStorageGet != nullptr) + { std::vector localTransform; - if (m_iFrameTransformStorageGet->getTransforms(localTransform)) { - return return_getAllTransforms(true, localTransform); + if (m_iFrameTransformStorageGet->getTransforms(localTransform)) + { + ret.transforms_list = localTransform; + ret.retvalue = ReturnValue_ok; + return ret; } } yCError(FRAMETRANSFORMGETNWSYARP) << "error getting transform from interface"; - return return_getAllTransforms(false, std::vector());; + ret.transforms_list = std::vector(); + ret.retvalue = ReturnValue::return_code::return_value_error_method_failed; + return ret; } void FrameTransformGet_nws_yarp::run() { std::lock_guard m_lock(m_mutex); + if (m_iFrameTransformStorageGet != nullptr) { std::vector localTransform; if (m_iFrameTransformStorageGet->getTransforms(localTransform)) { - return_getAllTransforms rgt(true, localTransform); + return_getAllTransforms rgt; + rgt.retvalue = ReturnValue_ok; + rgt.transforms_list = localTransform; m_streaming_port.write(rgt); } } diff --git a/src/devices/frameTransformSet/FrameTransformSetMultiplexer.cpp b/src/devices/frameTransformSet/FrameTransformSetMultiplexer.cpp index 6871897718..3b55aae253 100644 --- a/src/devices/frameTransformSet/FrameTransformSetMultiplexer.cpp +++ b/src/devices/frameTransformSet/FrameTransformSetMultiplexer.cpp @@ -78,38 +78,40 @@ bool FrameTransformSetMultiplexer::attachAll(const yarp::dev::PolyDriverList& de } -bool FrameTransformSetMultiplexer::setTransforms(const std::vector& transforms) +yarp::dev::ReturnValue FrameTransformSetMultiplexer::setTransforms(const std::vector& transforms) { for (size_t i = 0; i < m_iFrameTransformStorageSetList.size(); i++) { if (m_iFrameTransformStorageSetList[i] != nullptr) { m_iFrameTransformStorageSetList[i]->setTransforms(transforms); } - else { + else + { yCError(FRAMETRANSFORMSETMULTIPLEXER) << "pointer to interface IFrameTransformStorageSet not valid"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } } - return true; + return ReturnValue_ok; } -bool FrameTransformSetMultiplexer::setTransform(const yarp::math::FrameTransform& transform) +yarp::dev::ReturnValue FrameTransformSetMultiplexer::setTransform(const yarp::math::FrameTransform& transform) { for (size_t i = 0; i < m_iFrameTransformStorageSetList.size(); i++) { if (m_iFrameTransformStorageSetList[i] != nullptr) { m_iFrameTransformStorageSetList[i]->setTransform(transform); } - else { + else + { yCError(FRAMETRANSFORMSETMULTIPLEXER) << "pointer to interface IFrameTransformStorageSet not valid"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } } - return true; + return ReturnValue_ok; } -bool FrameTransformSetMultiplexer::deleteTransform(std::string t1, std::string t2) +yarp::dev::ReturnValue FrameTransformSetMultiplexer::deleteTransform(std::string t1, std::string t2) { //stopThreads(); @@ -121,18 +123,21 @@ bool FrameTransformSetMultiplexer::deleteTransform(std::string t1, std::string t { frame_deleted &= m_iFrameTransformStorageSetList[i]->deleteTransform(t1,t2); } - else { + else + { yCError(FRAMETRANSFORMSETMULTIPLEXER) << "pointer to interface IFrameTransformStorageSet not valid"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } } //startThreads(); - return frame_deleted; + if (frame_deleted) { return ReturnValue_ok; } + + return ReturnValue::return_code::return_value_error_method_failed; } -bool FrameTransformSetMultiplexer::clearAll() +yarp::dev::ReturnValue FrameTransformSetMultiplexer::clearAll() { //stopThreads(); @@ -141,15 +146,16 @@ bool FrameTransformSetMultiplexer::clearAll() if (m_iFrameTransformStorageSetList[i] != nullptr) { m_iFrameTransformStorageSetList[i]->clearAll(); } - else { + else + { yCError(FRAMETRANSFORMSETMULTIPLEXER) << "pointer to interface IFrameTransformStorageSet not valid"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } } //startThreads(); - return true; + return ReturnValue_ok; } void FrameTransformSetMultiplexer::stopThreads() diff --git a/src/devices/frameTransformSet/FrameTransformSetMultiplexer.h b/src/devices/frameTransformSet/FrameTransformSetMultiplexer.h index 82c1ec547e..ff6addc824 100644 --- a/src/devices/frameTransformSet/FrameTransformSetMultiplexer.h +++ b/src/devices/frameTransformSet/FrameTransformSetMultiplexer.h @@ -41,10 +41,10 @@ class FrameTransformSetMultiplexer: bool detachAll() override; // yarp::dev::IFrameTransformStorageSet - bool setTransform(const yarp::math::FrameTransform& transform) override; - bool setTransforms(const std::vector& transforms) override; - virtual bool deleteTransform(std::string t1, std::string t2) override; - virtual bool clearAll() override; + yarp::dev::ReturnValue setTransform(const yarp::math::FrameTransform& transform) override; + yarp::dev::ReturnValue setTransforms(const std::vector& transforms) override; + yarp::dev::ReturnValue deleteTransform(std::string t1, std::string t2) override; + yarp::dev::ReturnValue clearAll() override; void startThreads(); void stopThreads(); diff --git a/src/devices/frameTransformSet/FrameTransformSet_nwc_yarp.cpp b/src/devices/frameTransformSet/FrameTransformSet_nwc_yarp.cpp index 66f306f093..ccc7c1dec5 100644 --- a/src/devices/frameTransformSet/FrameTransformSet_nwc_yarp.cpp +++ b/src/devices/frameTransformSet/FrameTransformSet_nwc_yarp.cpp @@ -91,46 +91,54 @@ bool FrameTransformSet_nwc_yarp::close() return true; } -bool FrameTransformSet_nwc_yarp::setTransform(const yarp::math::FrameTransform& transform) +ReturnValue FrameTransformSet_nwc_yarp::setTransform(const yarp::math::FrameTransform& transform) { std::lock_guard lg(m_pd_mutex); - if(!m_setRPC.setTransformRPC(transform)) + + auto ret = m_setRPC.setTransformRPC(transform); + if(!ret) { yCError(FRAMETRANSFORMSETNWCYARP, "Unable to set transformation"); - return false; + return ret; } - return true; + return ret; } -bool FrameTransformSet_nwc_yarp::setTransforms(const std::vector& transforms) +ReturnValue FrameTransformSet_nwc_yarp::setTransforms(const std::vector& transforms) { std::lock_guard lg(m_pd_mutex); - if(!m_setRPC.setTransformsRPC(transforms)) + + auto ret = m_setRPC.setTransformsRPC(transforms); + if(!ret) { yCError(FRAMETRANSFORMSETNWCYARP, "Unable to set transformations"); - return false; + return ret; } - return true; + return ret; } -bool FrameTransformSet_nwc_yarp::deleteTransform(std::string t1, std::string t2) +ReturnValue FrameTransformSet_nwc_yarp::deleteTransform(std::string t1, std::string t2) { std::lock_guard lg(m_pd_mutex); - if (!m_setRPC.deleteTransformRPC(t1,t2)) + + auto ret = m_setRPC.deleteTransformRPC(t1, t2); + if (!ret) { yCError(FRAMETRANSFORMSETNWCYARP, "Unable to delete transformation"); - return false; + return ret; } - return true; + return ret; } -bool FrameTransformSet_nwc_yarp::clearAll() +ReturnValue FrameTransformSet_nwc_yarp::clearAll() { std::lock_guard lg(m_pd_mutex); - if (!m_setRPC.clearAllRPC()) + + auto ret = m_setRPC.clearAllRPC(); + if (!ret) { yCError(FRAMETRANSFORMSETNWCYARP, "Unable to clear all transformations"); - return false; + return ret; } - return true; + return ret; } diff --git a/src/devices/frameTransformSet/FrameTransformSet_nwc_yarp.h b/src/devices/frameTransformSet/FrameTransformSet_nwc_yarp.h index acc64addd3..3b66f3c3ce 100644 --- a/src/devices/frameTransformSet/FrameTransformSet_nwc_yarp.h +++ b/src/devices/frameTransformSet/FrameTransformSet_nwc_yarp.h @@ -80,10 +80,10 @@ class FrameTransformSet_nwc_yarp : bool close() override; //FrameTransformStorageSetRPC functions - bool setTransforms(const std::vector& transforms) override; - bool setTransform(const yarp::math::FrameTransform& transform) override; - bool deleteTransform(std::string t1, std::string t2) override; - bool clearAll() override; + yarp::dev::ReturnValue setTransforms(const std::vector& transforms) override; + yarp::dev::ReturnValue setTransform(const yarp::math::FrameTransform& transform) override; + yarp::dev::ReturnValue deleteTransform(std::string t1, std::string t2) override; + yarp::dev::ReturnValue clearAll() override; private: mutable std::mutex m_trf_mutex; diff --git a/src/devices/frameTransformSet/FrameTransformSet_nws_yarp.cpp b/src/devices/frameTransformSet/FrameTransformSet_nws_yarp.cpp index fdf97bb18b..0cb16ce7fd 100644 --- a/src/devices/frameTransformSet/FrameTransformSet_nws_yarp.cpp +++ b/src/devices/frameTransformSet/FrameTransformSet_nws_yarp.cpp @@ -80,53 +80,60 @@ bool FrameTransformSet_nws_yarp::detach() return true; } -bool FrameTransformSet_nws_yarp::setTransformRPC(const yarp::math::FrameTransform& transform) +ReturnValue FrameTransformSet_nws_yarp::setTransformRPC(const yarp::math::FrameTransform& transform) { std::lock_guard lg(m_pd_mutex); - if(!m_iSetIf->setTransform(transform)) + auto ret = m_iSetIf->setTransform(transform); + if(!ret) { yCError(FRAMETRANSFORMSETNWSYARP, "Unable to set transform"); - return false; + return ret; } - return true; + return ret; } -bool FrameTransformSet_nws_yarp::setTransformsRPC(const std::vector& transforms) +ReturnValue FrameTransformSet_nws_yarp::setTransformsRPC(const std::vector& transforms) { std::lock_guard lg(m_pd_mutex); - if(!m_iSetIf->setTransforms(transforms)) + + auto ret = m_iSetIf->setTransforms(transforms); + if(!ret) { yCError(FRAMETRANSFORMSETNWSYARP, "Unable to set transformations"); - return false; + return ret; } - return true; + return ret; } -bool FrameTransformSet_nws_yarp::deleteTransformRPC(const std::string& src, const std::string& dst) +ReturnValue FrameTransformSet_nws_yarp::deleteTransformRPC(const std::string& src, const std::string& dst) { std::lock_guard lg(m_pd_mutex); - if (!m_iSetIf->deleteTransform(src,dst)) + + auto ret = m_iSetIf->deleteTransform(src, dst); + if (!ret) { yCError(FRAMETRANSFORMSETNWSYARP, "Unable to delete transforms"); - return false; + return ret; } - return true; + return ret; } -bool FrameTransformSet_nws_yarp::clearAllRPC() +ReturnValue FrameTransformSet_nws_yarp::clearAllRPC() { std::lock_guard lg(m_pd_mutex); - if (!m_iSetIf->clearAll()) + + auto ret = m_iSetIf->clearAll(); + if (!ret) { yCError(FRAMETRANSFORMSETNWSYARP, "Unable to clear all transforms"); - return false; + return ret; } - return true; + return ret; } diff --git a/src/devices/frameTransformSet/FrameTransformSet_nws_yarp.h b/src/devices/frameTransformSet/FrameTransformSet_nws_yarp.h index c214740179..4507acd93f 100644 --- a/src/devices/frameTransformSet/FrameTransformSet_nws_yarp.h +++ b/src/devices/frameTransformSet/FrameTransformSet_nws_yarp.h @@ -73,10 +73,10 @@ class FrameTransformSet_nws_yarp : bool detach() override; //FrameTransformStorageSetRPC functions - bool setTransformsRPC(const std::vector& transforms) override; - bool setTransformRPC(const yarp::math::FrameTransform& transform) override; - bool deleteTransformRPC(const std::string& src, const std::string& dst) override; - bool clearAllRPC()override; + yarp::dev::ReturnValue setTransformsRPC(const std::vector& transforms) override; + yarp::dev::ReturnValue setTransformRPC(const yarp::math::FrameTransform& transform) override; + yarp::dev::ReturnValue deleteTransformRPC(const std::string& src, const std::string& dst) override; + yarp::dev::ReturnValue clearAllRPC()override; private: mutable std::mutex m_pd_mutex; diff --git a/src/devices/frameTransformStorage/FrameTransformStorage.cpp b/src/devices/frameTransformStorage/FrameTransformStorage.cpp index d582024f68..1be5f54c6a 100644 --- a/src/devices/frameTransformStorage/FrameTransformStorage.cpp +++ b/src/devices/frameTransformStorage/FrameTransformStorage.cpp @@ -18,10 +18,10 @@ YARP_LOG_COMPONENT(FRAMETRANSFORSTORAGE, "yarp.device.frameTransformStorage") } //------------------------------------------------------------------------------------------------------------------------------ -bool FrameTransformStorage::getInternalContainer(FrameTransformContainer*& container) +yarp::dev::ReturnValue FrameTransformStorage::getInternalContainer(FrameTransformContainer*& container) { container = &m_tf_container; - return true; + return ReturnValue_ok; } bool FrameTransformStorage::open(yarp::os::Searchable& config) @@ -52,25 +52,25 @@ bool FrameTransformStorage::close() return true; } -bool FrameTransformStorage::getTransforms(std::vector& transforms) const +yarp::dev::ReturnValue FrameTransformStorage::getTransforms(std::vector& transforms) const { std::lock_guard lg(m_pd_mutex); return m_tf_container.getTransforms(transforms); } -bool FrameTransformStorage::setTransforms(const std::vector& transforms) +yarp::dev::ReturnValue FrameTransformStorage::setTransforms(const std::vector& transforms) { std::lock_guard lg(m_pd_mutex); return m_tf_container.setTransforms(transforms); } -bool FrameTransformStorage::setTransform(const yarp::math::FrameTransform& t) +yarp::dev::ReturnValue FrameTransformStorage::setTransform(const yarp::math::FrameTransform& t) { std::lock_guard lg(m_pd_mutex); return m_tf_container.setTransform (t); } -bool FrameTransformStorage::deleteTransform(std::string t1, std::string t2) +yarp::dev::ReturnValue FrameTransformStorage::deleteTransform(std::string t1, std::string t2) { std::lock_guard lg(m_pd_mutex); return m_tf_container.deleteTransform(t1,t2); @@ -123,22 +123,30 @@ bool FrameTransformStorage::attach(yarp::dev::PolyDriver* driver) return false; } -bool FrameTransformStorage::clearAll() +yarp::dev::ReturnValue FrameTransformStorage::clearAll() { return m_tf_container.clearAll(); } -bool FrameTransformStorage::size(size_t& size) const +yarp::dev::ReturnValue FrameTransformStorage::size(size_t& size) const { - return m_tf_container.size(size); + if (m_tf_container.size(size)) + { + return ReturnValue_ok; + } + return ReturnValue::return_code::return_value_error_method_failed; } -bool FrameTransformStorage::startStorageThread() +yarp::dev::ReturnValue FrameTransformStorage::startStorageThread() { - return this->start(); + if (this->start()) + { + return ReturnValue_ok; + } + return ReturnValue::return_code::return_value_error_method_failed; } -bool FrameTransformStorage::stopStorageThread() +yarp::dev::ReturnValue FrameTransformStorage::stopStorageThread() { this->askToStop(); do @@ -146,5 +154,5 @@ bool FrameTransformStorage::stopStorageThread() yarp::os::Time::delay(0.001); } while (this->isRunning()); - return true; + return ReturnValue_ok; } diff --git a/src/devices/frameTransformStorage/FrameTransformStorage.h b/src/devices/frameTransformStorage/FrameTransformStorage.h index 3e4b5dbfc3..e4b809fdef 100644 --- a/src/devices/frameTransformStorage/FrameTransformStorage.h +++ b/src/devices/frameTransformStorage/FrameTransformStorage.h @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -55,19 +56,19 @@ class FrameTransformStorage : bool close() override; //IFrameTransformStorageSet interface - bool setTransforms(const std::vector& transforms) override; - bool setTransform(const yarp::math::FrameTransform& transform) override; + yarp::dev::ReturnValue setTransforms(const std::vector& transforms) override; + yarp::dev::ReturnValue setTransform(const yarp::math::FrameTransform& transform) override; //IFrameTransformStorageGet interface - bool getTransforms(std::vector& transforms) const override; + yarp::dev::ReturnValue getTransforms(std::vector& transforms) const override; //IFrameTransformStorageUtils interface - bool deleteTransform(std::string t1, std::string t2) override; - bool size(size_t& size) const override; - bool clearAll() override; - bool getInternalContainer(yarp::dev::FrameTransformContainer*& container) override; - bool startStorageThread() override; - bool stopStorageThread() override; + yarp::dev::ReturnValue deleteTransform(std::string t1, std::string t2) override; + yarp::dev::ReturnValue size(size_t& size) const override; + yarp::dev::ReturnValue clearAll() override; + yarp::dev::ReturnValue getInternalContainer(yarp::dev::FrameTransformContainer*& container) override; + yarp::dev::ReturnValue startStorageThread() override; + yarp::dev::ReturnValue stopStorageThread() override; //wrapper and interfaces bool attach(yarp::dev::PolyDriver* driver) override; diff --git a/src/devices/messages/frameTransformStorageMsgs/CMakeLists.txt b/src/devices/messages/frameTransformStorageMsgs/CMakeLists.txt index d5533f3654..d50b0a44ec 100644 --- a/src/devices/messages/frameTransformStorageMsgs/CMakeLists.txt +++ b/src/devices/messages/frameTransformStorageMsgs/CMakeLists.txt @@ -15,6 +15,7 @@ target_sources(frameTransformStorageMsgs PRIVATE ${FTM_THRIFT_GEN_FILES}) target_link_libraries(frameTransformStorageMsgs PRIVATE YARP::YARP_os + YARP::YARP_dev YARP::YARP_sig YARP::YARP_math ) diff --git a/src/devices/messages/frameTransformStorageMsgs/frameTransformStorageMsgs.thrift b/src/devices/messages/frameTransformStorageMsgs/frameTransformStorageMsgs.thrift index 678ab8072f..7365f8ee05 100644 --- a/src/devices/messages/frameTransformStorageMsgs/frameTransformStorageMsgs.thrift +++ b/src/devices/messages/frameTransformStorageMsgs/frameTransformStorageMsgs.thrift @@ -9,18 +9,24 @@ struct YarpFrameTransform { yarp.includefile="yarp/math/FrameTransform.h" ) +struct yReturnValue { +} ( + yarp.name = "yarp::dev::ReturnValue" + yarp.includefile = "yarp/dev/ReturnValue.h" +) + struct return_getAllTransforms { - 1: bool retvalue; + 1: yReturnValue retvalue; 2: list transforms_list; } service FrameTransformStorageSetRPC { - bool setTransformsRPC(1:list transforms) ; - bool setTransformRPC(1:YarpFrameTransform transform); - bool deleteTransformRPC(1:string src, 2:string dst); - bool clearAllRPC() + yReturnValue setTransformsRPC(1:list transforms) ; + yReturnValue setTransformRPC(1:YarpFrameTransform transform); + yReturnValue deleteTransformRPC(1:string src, 2:string dst); + yReturnValue clearAllRPC() } service FrameTransformStorageGetRPC diff --git a/src/libYARP_dev/src/yarp/dev/FrameTransformContainer.cpp b/src/libYARP_dev/src/yarp/dev/FrameTransformContainer.cpp index c57c1e3d59..a18a011e85 100644 --- a/src/libYARP_dev/src/yarp/dev/FrameTransformContainer.cpp +++ b/src/libYARP_dev/src/yarp/dev/FrameTransformContainer.cpp @@ -35,25 +35,25 @@ void FrameTransformContainer::invalidateTransform(yarp::math::FrameTransform& tr } } -bool FrameTransformContainer::getTransforms(std::vector& transforms) const +ReturnValue FrameTransformContainer::getTransforms(std::vector& transforms) const { std::lock_guard lock(m_trf_mutex); std::copy_if (m_transforms.begin(), m_transforms.end(), std::back_inserter(transforms), [](const yarp::math::FrameTransform& trf){return trf.isValid();}); - return true; + return ReturnValue_ok; } -bool FrameTransformContainer::setTransforms(const std::vector& transforms) +ReturnValue FrameTransformContainer::setTransforms(const std::vector& transforms) { for (auto& it : transforms) { setTransform(it); } - return true; + return ReturnValue_ok; } -bool FrameTransformContainer::setTransform(const yarp::math::FrameTransform& new_tr) +ReturnValue FrameTransformContainer::setTransform(const yarp::math::FrameTransform& new_tr) { - if (new_tr.isValid()==false) return true; + if (new_tr.isValid()==false) return ReturnValue_ok; std::lock_guard lock(m_trf_mutex); for (auto& it : m_transforms) @@ -69,27 +69,27 @@ bool FrameTransformContainer::setTransform(const yarp::math::FrameTransform& new if (new_tr.timestamp > it.timestamp) { it = new_tr; - return true; + return ReturnValue_ok; } else { //yCDebug(FRAMETRANSFORMCONTAINER) << "Received old transform" << it.dst_frame_id << it.src_frame_id << std::to_string(it.timestamp) << it.isStatic; - return true; + return ReturnValue_ok; } } else { - return true; + return ReturnValue_ok; } } } //add a new transform m_transforms.push_back(new_tr); - return true; + return ReturnValue_ok; } -bool FrameTransformContainer::deleteTransform(std::string t1, std::string t2) +ReturnValue FrameTransformContainer::deleteTransform(std::string t1, std::string t2) { std::lock_guard lock(m_trf_mutex); if (t1 == "*" && t2 == "*") @@ -98,7 +98,7 @@ bool FrameTransformContainer::deleteTransform(std::string t1, std::string t2) { invalidateTransform(m_transforms[i]); } - return true; + return ReturnValue_ok; } else { @@ -112,7 +112,7 @@ bool FrameTransformContainer::deleteTransform(std::string t1, std::string t2) invalidateTransform(m_transforms[i]); } } - return true; + return ReturnValue_ok; } else { @@ -126,7 +126,7 @@ bool FrameTransformContainer::deleteTransform(std::string t1, std::string t2) invalidateTransform(m_transforms[i]); } } - return true; + return ReturnValue_ok; } else { @@ -136,7 +136,7 @@ bool FrameTransformContainer::deleteTransform(std::string t1, std::string t2) (m_transforms[i].dst_frame_id == t2 && m_transforms[i].src_frame_id == t1)) { invalidateTransform(m_transforms[i]); - return true; + return ReturnValue_ok; } } } @@ -144,17 +144,17 @@ bool FrameTransformContainer::deleteTransform(std::string t1, std::string t2) } yCError(FRAMETRANSFORMCONTAINER) << "Transformation deletion not successful"; - return false; + return ReturnValue::return_code::return_value_error_method_failed; } -bool FrameTransformContainer::clearAll() +ReturnValue FrameTransformContainer::clearAll() { std::lock_guard lock(m_trf_mutex); for (size_t i = 0; i < m_transforms.size(); i++) { invalidateTransform(m_transforms[i]); } - return true; + return ReturnValue_ok; } bool FrameTransformContainer::checkAndRemoveExpired() @@ -180,7 +180,7 @@ bool FrameTransformContainer::checkAndRemoveExpired() goto check_vector; } } - return true; + return ReturnValue_ok; } bool FrameTransformContainer::checkAndRemoveExpired() const diff --git a/src/libYARP_dev/src/yarp/dev/FrameTransformContainer.h b/src/libYARP_dev/src/yarp/dev/FrameTransformContainer.h index 0e0778d377..b8acf94d36 100644 --- a/src/libYARP_dev/src/yarp/dev/FrameTransformContainer.h +++ b/src/libYARP_dev/src/yarp/dev/FrameTransformContainer.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -105,15 +106,15 @@ class YARP_dev_API FrameTransformContainer : ~FrameTransformContainer() {} //IFrameTransformStorageSet interface - bool setTransforms(const std::vector& transforms) override; - bool setTransform(const yarp::math::FrameTransform& transform) override; + yarp::dev::ReturnValue setTransforms(const std::vector& transforms) override; + yarp::dev::ReturnValue setTransform(const yarp::math::FrameTransform& transform) override; //IFrameTransformStorageGet interface - bool getTransforms(std::vector& transforms) const override; + yarp::dev::ReturnValue getTransforms(std::vector& transforms) const override; //IFrameTransformStorageUtils interface - bool deleteTransform(std::string t1, std::string t2) override; - bool clearAll() override; + yarp::dev::ReturnValue deleteTransform(std::string t1, std::string t2) override; + yarp::dev::ReturnValue clearAll() override; bool size(size_t& size) const; diff --git a/src/libYARP_dev/src/yarp/dev/IFrameTransform.h b/src/libYARP_dev/src/yarp/dev/IFrameTransform.h index 7806ba06bd..4b0106df1f 100644 --- a/src/libYARP_dev/src/yarp/dev/IFrameTransform.h +++ b/src/libYARP_dev/src/yarp/dev/IFrameTransform.h @@ -14,6 +14,7 @@ #include #include #include +#include namespace yarp::dev { class IFrameTransform; @@ -43,37 +44,39 @@ class YARP_dev_API yarp::dev::IFrameTransform * @param all_frames the returned string containing the frames * @return true/false */ - virtual bool allFramesAsString(std::string &all_frames) = 0; + virtual yarp::dev::ReturnValue allFramesAsString(std::string &all_frames) = 0; /** Test if a transform exists. * @param target_frame_id the name of target reference frame * @param source_frame_id the name of source reference frame + * @return canTransform true if the transformation exists, false otherwise * @return true/false */ - virtual bool canTransform (const std::string &target_frame, const std::string &source_frame) = 0; + virtual yarp::dev::ReturnValue canTransform (const std::string &target_frame, const std::string &source_frame, bool& canTransform) = 0; /** Removes all the registered transforms. * @return true/false */ - virtual bool clear () = 0; + virtual yarp::dev::ReturnValue clear () = 0; /** Check if a frame exists. * @param frame_id the frame to be searched * @param target_frame_id the name of target reference frame * @param source_frame_id the name of source reference frame + * @return exists true if the transformation exists, false otherwise * @return true/false */ - virtual bool frameExists (const std::string &frame_id) = 0; + virtual yarp::dev::ReturnValue frameExists (const std::string &frame_id, bool& exists) = 0; /** Gets a vector containing all the registered frames. * @param ids the returned vector containing all frame ids * @return true/false */ - virtual bool getAllFrameIds (std::vector< std::string > &ids) = 0; + virtual yarp::dev::ReturnValue getAllFrameIds (std::vector< std::string > &ids) = 0; /** Get the parent of a frame. @@ -81,7 +84,7 @@ class YARP_dev_API yarp::dev::IFrameTransform * @param parent_frame_id the name of parent reference frame * @return true/false */ - virtual bool getParent (const std::string &frame_id, std::string &parent_frame_id) = 0; + virtual yarp::dev::ReturnValue getParent (const std::string &frame_id, std::string &parent_frame_id) = 0; /** Get the transform between two frames. @@ -90,7 +93,7 @@ class YARP_dev_API yarp::dev::IFrameTransform * @param transform the transformation matrix from source_frame_id to target_frame_id * @return true/false */ - virtual bool getTransform (const std::string &target_frame_id, const std::string &source_frame_id, yarp::sig::Matrix &transform) = 0; + virtual yarp::dev::ReturnValue getTransform (const std::string &target_frame_id, const std::string &source_frame_id, yarp::sig::Matrix &transform) = 0; /** Register a transform between two frames. @@ -99,7 +102,7 @@ class YARP_dev_API yarp::dev::IFrameTransform * @param transform the transformation matrix from source_frame_id to target_frame_id * @return true/false */ - virtual bool setTransform (const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) = 0; + virtual yarp::dev::ReturnValue setTransform (const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) = 0; /** Register a static transform between two frames. @@ -108,7 +111,7 @@ class YARP_dev_API yarp::dev::IFrameTransform * @param transform the transformation matrix from source_frame_id to target_frame_id * @return true/false */ - virtual bool setTransformStatic(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) = 0; + virtual yarp::dev::ReturnValue setTransformStatic(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) = 0; /** Deletes a transform between two frames. @@ -116,7 +119,7 @@ class YARP_dev_API yarp::dev::IFrameTransform * @param source_frame_id the name of source reference frame * @return true/false */ - virtual bool deleteTransform (const std::string &target_frame_id, const std::string &source_frame_id) = 0; + virtual yarp::dev::ReturnValue deleteTransform (const std::string &target_frame_id, const std::string &source_frame_id) = 0; /** Transform a point into the target frame. @@ -126,7 +129,7 @@ class YARP_dev_API yarp::dev::IFrameTransform * @param transformed_point the returned point (x y z) * @return true/false */ - virtual bool transformPoint (const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_point, yarp::sig::Vector &transformed_point) = 0; + virtual yarp::dev::ReturnValue transformPoint (const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_point, yarp::sig::Vector &transformed_point) = 0; /** Transform a Stamped Pose into the target frame. @@ -136,7 +139,7 @@ class YARP_dev_API yarp::dev::IFrameTransform * @param transformed_pose the returned (x y z r p y) * @return true/false */ - virtual bool transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose) = 0; + virtual yarp::dev::ReturnValue transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose) = 0; /** Transform a quaternion into the target frame. @@ -146,7 +149,7 @@ class YARP_dev_API yarp::dev::IFrameTransform * @param transformed_quaternion the returned quaternion (x y z w) * @return true/false */ - virtual bool transformQuaternion(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::math::Quaternion &input_quaternion, yarp::math::Quaternion &transformed_quaternion) = 0; + virtual yarp::dev::ReturnValue transformQuaternion(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::math::Quaternion &input_quaternion, yarp::math::Quaternion &transformed_quaternion) = 0; /** Block until a transform from source_frame_id to target_frame_id is possible or it times out. @@ -156,7 +159,7 @@ class YARP_dev_API yarp::dev::IFrameTransform * @param error_msg string filled with error message (if error occurred) * @return true/false */ - virtual bool waitForTransform(const std::string &target_frame_id, const std::string &source_frame_id, const double &timeout) = 0; + virtual yarp::dev::ReturnValue waitForTransform(const std::string &target_frame_id, const std::string &source_frame_id, const double &timeout) = 0; }; constexpr yarp::conf::vocab32_t VOCAB_ITRANSFORM = yarp::os::createVocab32('i','t','r','f'); diff --git a/src/libYARP_dev/src/yarp/dev/IFrameTransformStorage.h b/src/libYARP_dev/src/yarp/dev/IFrameTransformStorage.h index 6cda6fa877..7875c49751 100644 --- a/src/libYARP_dev/src/yarp/dev/IFrameTransformStorage.h +++ b/src/libYARP_dev/src/yarp/dev/IFrameTransformStorage.h @@ -12,6 +12,7 @@ #include #include #include +#include namespace yarp::dev { class IFrameTransformStorageSet; @@ -35,20 +36,20 @@ class YARP_dev_API yarp::dev::IFrameTransformStorageSet * @param transforms the list of transforms to be stored * @return true/false */ - virtual bool setTransforms(const std::vector& transforms) = 0; + virtual yarp::dev::ReturnValue setTransforms(const std::vector& transforms) = 0; /** * Save a frame transform in a storage. * @param transforms the transform to be stored * @return true/false */ - virtual bool setTransform(const yarp::math::FrameTransform& transform) = 0; + virtual yarp::dev::ReturnValue setTransform(const yarp::math::FrameTransform& transform) = 0; /** * Delete all transforms in a storage. * @return true/false */ - virtual bool clearAll() = 0; + virtual yarp::dev::ReturnValue clearAll() = 0; /** * Delete a single transform in the storage. @@ -56,7 +57,7 @@ class YARP_dev_API yarp::dev::IFrameTransformStorageSet * @param dst the destination of frame transform to delete * @return true/false */ - virtual bool deleteTransform(std::string src, std::string dst) = 0; + virtual yarp::dev::ReturnValue deleteTransform(std::string src, std::string dst) = 0; }; /** @@ -74,7 +75,7 @@ class YARP_dev_API yarp::dev::IFrameTransformStorageGet * @param transforms the returned list of frame transforms * @return true/false */ - virtual bool getTransforms(std::vector& transforms) const = 0; + virtual yarp::dev::ReturnValue getTransforms(std::vector& transforms) const = 0; }; /** @@ -87,13 +88,13 @@ class YARP_dev_API yarp::dev::IFrameTransformStorageUtils public: virtual ~IFrameTransformStorageUtils(); - virtual bool size (size_t& size) const =0; + virtual yarp::dev::ReturnValue size (size_t& size) const =0; - virtual bool getInternalContainer(yarp::dev::FrameTransformContainer*& container) =0; + virtual yarp::dev::ReturnValue getInternalContainer(yarp::dev::FrameTransformContainer*& container) =0; - virtual bool startStorageThread() = 0; + virtual yarp::dev::ReturnValue startStorageThread() = 0; - virtual bool stopStorageThread() = 0; + virtual yarp::dev::ReturnValue stopStorageThread() = 0; }; #endif // YARP_DEV_IFRAMETRANSFORM_STORAGE_H diff --git a/src/libYARP_dev/src/yarp/dev/ReturnValue.cpp b/src/libYARP_dev/src/yarp/dev/ReturnValue.cpp index 19fc1a8638..c51a3571a5 100644 --- a/src/libYARP_dev/src/yarp/dev/ReturnValue.cpp +++ b/src/libYARP_dev/src/yarp/dev/ReturnValue.cpp @@ -89,8 +89,6 @@ std::string ReturnValue::toString() { case return_code::return_value_ok: return std::string("ok"); - case return_code::return_value_uninitialized: - return std::string("return_value_uninitialized"); case return_code::return_value_error_deprecated: return std::string("return_value_error_deprecated"); case return_code::return_value_error_generic: @@ -101,8 +99,11 @@ std::string ReturnValue::toString() return std::string("return_value_error_not_implemented_by_device"); case return_code::return_value_error_nws_nwc_communication_error: return std::string("return_value_error_nws_nwc_communication_error"); + case return_code::return_value_error_not_ready: + return std::string("return_value_error_not_ready"); default: - return std::string("unknown"); + case return_code::return_value_uninitialized: + return std::string("unknown/uninitialized"); } } diff --git a/src/libYARP_dev/src/yarp/dev/ReturnValue.h b/src/libYARP_dev/src/yarp/dev/ReturnValue.h index 3e4eba585a..cd1a8509d7 100644 --- a/src/libYARP_dev/src/yarp/dev/ReturnValue.h +++ b/src/libYARP_dev/src/yarp/dev/ReturnValue.h @@ -37,6 +37,7 @@ class YARP_dev_API ReturnValue : public yarp::os::Portable return_value_error_nws_nwc_communication_error = 3, /// Command answer lost during network transmission. Status unknown. return_value_error_deprecated = 4, /// Method is deprecated return_value_error_method_failed = 5, /// Method failed due to invalid internal status/invalid request + return_value_error_not_ready = 6, /// Method failed because some initialization is missing return_value_uninitialized = 100 /// Default value, should never be explicitly assigned }; diff --git a/src/libYARP_dev/src/yarp/dev/tests/IFrameTransformTest.h b/src/libYARP_dev/src/yarp/dev/tests/IFrameTransformTest.h index c9d9f762f9..6a5585778c 100644 --- a/src/libYARP_dev/src/yarp/dev/tests/IFrameTransformTest.h +++ b/src/libYARP_dev/src/yarp/dev/tests/IFrameTransformTest.h @@ -167,17 +167,23 @@ namespace yarp::dev::tests //test3 { + bool exists = false; yInfo() << "Running Sub-test3"; - CHECK(itf->frameExists("frame3")); - CHECK_FALSE(itf->frameExists("frame3_err")); // frameExists ok + CHECK(itf->frameExists("frame3", exists)); + CHECK(exists); + CHECK(itf->frameExists("frame3_err",exists)); // frameExists ok + CHECK(!exists); yInfo() << "Sub-test complete"; } //test4 { + bool canTransform = false; yInfo() << "Running Sub-test4"; - CHECK(itf->canTransform("frame2", "frame1")); - CHECK_FALSE(itf->canTransform("frame11", "frame1")); // canTransform ok + CHECK(itf->canTransform("frame2", "frame1", canTransform)); + CHECK(canTransform); + CHECK(itf->canTransform("frame11", "frame1", canTransform)); // canTransform ok + CHECK(!canTransform); yInfo() << "Sub-test complete"; } @@ -185,18 +191,22 @@ namespace yarp::dev::tests { yInfo() << "Running Sub-test4b"; bool b_canb1; - b_canb1 = itf->canTransform("frame3b", "frame1"); + bool canTransform = false; + b_canb1 = itf->canTransform("frame3b", "frame1", canTransform); CHECK(b_canb1); // canTransform Bis ok + CHECK(canTransform); yInfo() << "Sub-test complete"; } - //test4 tris (transform between sibilings) + //test4tris (transform between siblings) { yInfo() << "Running Sub-test4c"; yarp::sig::Matrix sib; - CHECK(itf->canTransform("sibiling_test_frame", "frame3")); // canTransform between sibilings ok - CHECK(itf->getTransform("sibiling_test_frame", "frame3", sib)); // getTransform between sibilings ok - CHECK(isEqual(sib, SE3inv(m2) * SE3inv(m1) * sibiling, precision)); // transform between sibilings ok + bool canTransform = false; + CHECK(itf->canTransform("sibiling_test_frame", "frame3",canTransform)); // canTransform between siblings ok + CHECK(canTransform); + CHECK(itf->getTransform("sibiling_test_frame", "frame3", sib)); // getTransform between siblings ok + CHECK(isEqual(sib, SE3inv(m2) * SE3inv(m1) * sibiling, precision)); // transform between siblings ok yInfo() << "Sub-test complete"; } @@ -290,18 +300,22 @@ namespace yarp::dev::tests yInfo() << "Running Sub-test8"; itf->setTransformStatic("frame_test", "frame1", m1); yarp::os::Time::delay(1); - bool del_bool = itf->frameExists("frame_test"); + bool exists = false; + bool del_bool = itf->frameExists("frame_test", exists); CHECK(del_bool); // frame exists + CHECK(exists); del_bool &= itf->deleteTransform("frame_test", "frame1"); CHECK(del_bool); // deleteTransform ok - del_bool &= (!itf->frameExists("frame_test")); + del_bool &= (itf->frameExists("frame_test", exists)); CHECK(del_bool); // check if frame has been successfully removed + CHECK(!exists); //let's wait some time and check gain to be sure that //frame does not reappears.... yarp::os::Time::delay(0.5); - del_bool &= (!itf->frameExists("frame_test")); + del_bool = itf->frameExists("frame_test", exists); CHECK(del_bool); + CHECK(!exists); //just print std::string strs; @@ -337,11 +351,14 @@ namespace yarp::dev::tests itf->setTransform("frame2", "frame10", m1); yarp::os::Time::delay(0.050); bool b_can; - b_can = itf->canTransform("frame2", "frame10"); + bool canTransform = false; + b_can = itf->canTransform("frame2", "frame10",canTransform); CHECK(b_can); // itf->setTransform ok + CHECK(canTransform); yarp::os::Time::delay(0.6); - b_can = itf->canTransform("frame2", "frame10"); - CHECK_FALSE(b_can); // itf->setTransform successfully expired after 0.6s + b_can = itf->canTransform("frame2", "frame10",canTransform); + CHECK(b_can); + CHECK(!canTransform); // itf->setTransform successfully expired after 0.6s yInfo() << "Sub-test complete"; } @@ -455,14 +472,18 @@ namespace yarp::dev::tests yInfo() << "Running Sub-test13"; itf->clear(); bool bcan = false; - bcan = itf->canTransform("not_existing_frame", "not_existing_frame"); + bool canTransform = false; + bcan = itf->canTransform("not_existing_frame", "not_existing_frame", canTransform); CHECK(bcan); + CHECK(canTransform); CHECK(itf->setTransformStatic("frame2", "frame1", m1)); - bcan = itf->canTransform("frame2", "frame2"); + bcan = itf->canTransform("frame2", "frame2", canTransform); CHECK(bcan); - bcan = itf->canTransform("frame1", "frame1"); + CHECK(canTransform); + bcan = itf->canTransform("frame1", "frame1", canTransform); CHECK(bcan); + CHECK(canTransform); yarp::sig::Matrix mt1; yarp::sig::Matrix eyemat(4, 4); eyemat.eye(); @@ -523,12 +544,18 @@ namespace yarp::dev::tests } // test canTransform - CHECK(itf->canTransform("/finger", "/arm")); - CHECK(!itf->canTransform("/finger", "/head")); + bool canTransform = false; + CHECK(itf->canTransform("/finger", "/arm", canTransform)); + CHECK(canTransform); + CHECK(itf->canTransform("/finger", "/head", canTransform)); + CHECK(!canTransform); // test frameExists - CHECK(itf->frameExists("/finger")); - CHECK(!itf->frameExists("/head")); + bool exists = false; + CHECK(itf->frameExists("/finger", exists)); + CHECK(exists); + CHECK(itf->frameExists("/head", exists)); + CHECK(!exists); // test getAllFrameIds CHECK(itf->clear());