From 83fcc981c5fd487045453677ad58749168f8ec3a Mon Sep 17 00:00:00 2001 From: elandini84 <62991576+elandini84@users.noreply.github.com> Date: Thu, 19 Jan 2023 16:11:54 +0100 Subject: [PATCH 1/6] Added checks for missing interfaces --- .../FrameTransformClient.cpp | 95 ++++++++++++++++--- 1 file changed, 81 insertions(+), 14 deletions(-) diff --git a/src/devices/frameTransformClient/FrameTransformClient.cpp b/src/devices/frameTransformClient/FrameTransformClient.cpp index 31713d92c9f..bc7f5a4b7ee 100644 --- a/src/devices/frameTransformClient/FrameTransformClient.cpp +++ b/src/devices/frameTransformClient/FrameTransformClient.cpp @@ -356,30 +356,32 @@ bool FrameTransformClient::open(yarp::os::Searchable &config) if (m_robot.hasParam("setDeviceName")) { setdeviceName = m_robot.findParam("setDeviceName");} if (!m_robot.hasDevice(setdeviceName)) { - yCError(FRAMETRANSFORMCLIENT) << "Failed to find requested device " << setdeviceName; - return false; + yCWarning(FRAMETRANSFORMCLIENT) << "Failed to find requested device " << setdeviceName; } - - auto* polyset = m_robot.device(setdeviceName).driver(); - if (!polyset || !polyset->view(m_ift_set)) + else { - yCError(FRAMETRANSFORMCLIENT) << "Failed to open device driver / interface for " << setdeviceName; - return false; + auto* polyset = m_robot.device(setdeviceName).driver(); + if (!polyset || !polyset->view(m_ift_set)) + { + yCError(FRAMETRANSFORMCLIENT) << "Failed to open device driver / interface for " << setdeviceName; + return false; + } } std::string getdeviceName = "ftc_storage"; if (m_robot.hasParam("getDeviceName")) {getdeviceName = m_robot.findParam("getDeviceName");} if (!m_robot.hasDevice(getdeviceName)) { - yCError(FRAMETRANSFORMCLIENT) << "Failed to find requested device " << getdeviceName; - return false; + yCWarning(FRAMETRANSFORMCLIENT) << "Failed to find requested device " << getdeviceName; } - - auto* polyget = m_robot.device(getdeviceName).driver(); - if (!polyget || !polyget->view(m_ift_get) || !polyget->view(m_ift_util)) + else { - yCError(FRAMETRANSFORMCLIENT) << "Failed to open device driver / interface for " << getdeviceName; - return false; + auto* polyget = m_robot.device(getdeviceName).driver(); + if (!polyget || !polyget->view(m_ift_get) || !polyget->view(m_ift_util)) + { + yCError(FRAMETRANSFORMCLIENT) << "Failed to open device driver / interface for " << getdeviceName; + return false; + } } if (config.check("period")) @@ -412,6 +414,11 @@ bool FrameTransformClient::close() bool 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; + } FrameTransformContainer* p_cont = nullptr; bool br = m_ift_util->getInternalContainer(p_cont); if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; } @@ -489,11 +496,21 @@ FrameTransformClient::ConnectionType FrameTransformClient::priv_getConnectionTyp bool FrameTransformClient::canTransform(const std::string &target_frame, const std::string &source_frame) { + if(!m_ift_util) + { + yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured", __func__); + return false; + } return priv_getConnectionType(target_frame, source_frame) != ConnectionType::DISCONNECTED; } bool FrameTransformClient::clear() { + if(!m_ift_set) + { + yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageSet interface found. Your device is wrongly configured",__func__); + return false; + } if (m_ift_set) { return m_ift_set->clearAll(); @@ -504,6 +521,11 @@ bool FrameTransformClient::clear() bool FrameTransformClient::frameExists(const std::string &frame_id) { + if(!m_ift_util) + { + yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured", __func__); + return false; + } FrameTransformContainer* p_cont = nullptr; bool br = m_ift_util->getInternalContainer(p_cont); if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; } @@ -524,6 +546,11 @@ bool FrameTransformClient::frameExists(const std::string &frame_id) bool FrameTransformClient::getAllFrameIds(std::vector &ids) { + if(!m_ift_util) + { + yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); + return false; + } FrameTransformContainer* p_cont = nullptr; bool br = m_ift_util->getInternalContainer(p_cont); if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; } @@ -561,6 +588,11 @@ bool FrameTransformClient::getAllFrameIds(std::vector &ids) bool 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; + } FrameTransformContainer* p_cont = nullptr; bool br = m_ift_util->getInternalContainer(p_cont); if (!br || p_cont == nullptr) { yCError(FRAMETRANSFORMCLIENT) << "Failure"; return false; } @@ -636,6 +668,11 @@ bool FrameTransformClient::priv_getChainedTransform(const std::string& target_fr bool 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; + } ConnectionType ct; std::string ancestor; ct = priv_getConnectionType(target_frame_id, source_frame_id, &ancestor); @@ -671,6 +708,11 @@ bool FrameTransformClient::getTransform(const std::string& target_frame_id, cons bool 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; + } if(target_frame_id == source_frame_id) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransform(): Invalid transform detected.\n" \ @@ -709,6 +751,11 @@ bool FrameTransformClient::setTransform(const std::string& target_frame_id, cons bool 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; + } if(target_frame_id == source_frame_id) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "setTransformStatic(): Invalid transform detected.\n" \ @@ -753,6 +800,11 @@ bool FrameTransformClient::deleteTransform(const std::string &target_frame_id, c 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) { + if(!m_ift_util) + { + yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); + return false; + } if (input_point.size() != 3) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "Only 3 dimensional vector allowed."; @@ -773,6 +825,11 @@ bool FrameTransformClient::transformPoint(const std::string &target_frame_id, co 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) { + if(!m_ift_util) + { + yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); + return false; + } if (input_pose.size() != 6) { yCErrorThrottle(FRAMETRANSFORMCLIENT, LOG_THROTTLE_PERIOD) << "Only 6 dimensional vector (3 axes + roll pith and yaw) allowed."; @@ -807,6 +864,11 @@ bool FrameTransformClient::transformPose(const std::string &target_frame_id, con 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) { + if(!m_ift_util) + { + yCError(FRAMETRANSFORMCLIENT, "%s: No IFrameTransformStorageUtils interface found. Your device is wrongly configured",__func__); + return false; + } yarp::sig::Matrix m(4, 4); if (!getTransform(target_frame_id, source_frame_id, m)) { @@ -821,6 +883,11 @@ bool FrameTransformClient::transformQuaternion(const std::string &target_frame_i bool 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; + } //loop until canTransform == true or timeout expires double start = yarp::os::SystemClock::nowSystem(); while (!canTransform(target_frame_id, source_frame_id)) From 5e16d5f6fb14f50108053258950be8e3a919d5d4 Mon Sep 17 00:00:00 2001 From: elandini84 <62991576+elandini84@users.noreply.github.com> Date: Thu, 19 Jan 2023 16:52:39 +0100 Subject: [PATCH 2/6] Added changelog --- .../master/fix_FrameTransformClient_interfaces_check.md | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 doc/release/master/fix_FrameTransformClient_interfaces_check.md diff --git a/doc/release/master/fix_FrameTransformClient_interfaces_check.md b/doc/release/master/fix_FrameTransformClient_interfaces_check.md new file mode 100644 index 00000000000..7057696950c --- /dev/null +++ b/doc/release/master/fix_FrameTransformClient_interfaces_check.md @@ -0,0 +1,8 @@ +fix/FrameTransformClient/interfaces_check {#master} +------------------- + +### Tools + +#### `frameTransformClient` + +* Now, if the selected configuration xml file does not include a certain interface, the device is still correctly initialized but that interface won't be available. If a method that involves the missing interface is called, an error will be returned. From 1a893abc597bc403163c2285fbec690c3903970e Mon Sep 17 00:00:00 2001 From: elandini84 <62991576+elandini84@users.noreply.github.com> Date: Fri, 20 Jan 2023 10:42:26 +0100 Subject: [PATCH 3/6] Fixed warning message on missing devices in the configuration file --- src/devices/frameTransformClient/FrameTransformClient.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/devices/frameTransformClient/FrameTransformClient.cpp b/src/devices/frameTransformClient/FrameTransformClient.cpp index bc7f5a4b7ee..779513aac78 100644 --- a/src/devices/frameTransformClient/FrameTransformClient.cpp +++ b/src/devices/frameTransformClient/FrameTransformClient.cpp @@ -356,7 +356,7 @@ bool FrameTransformClient::open(yarp::os::Searchable &config) if (m_robot.hasParam("setDeviceName")) { setdeviceName = m_robot.findParam("setDeviceName");} if (!m_robot.hasDevice(setdeviceName)) { - yCWarning(FRAMETRANSFORMCLIENT) << "Failed to find requested device " << setdeviceName; + yCWarning(FRAMETRANSFORMCLIENT) << "Set device name was not provided in the specified configuration. Set operations will not be available"; } else { @@ -372,7 +372,7 @@ bool FrameTransformClient::open(yarp::os::Searchable &config) if (m_robot.hasParam("getDeviceName")) {getdeviceName = m_robot.findParam("getDeviceName");} if (!m_robot.hasDevice(getdeviceName)) { - yCWarning(FRAMETRANSFORMCLIENT) << "Failed to find requested device " << getdeviceName; + yCWarning(FRAMETRANSFORMCLIENT) << "Get device name was not provided in the specified configuration. Get operations will not be available"; } else { From 335f9838bd8a265a68f9e05aef57ea2930b8c4a9 Mon Sep 17 00:00:00 2001 From: elandini84 <62991576+elandini84@users.noreply.github.com> Date: Fri, 20 Jan 2023 11:19:27 +0100 Subject: [PATCH 4/6] Check for device in configuration file improved Now if no device name (for set or get interfaces) is specificed a warning will be returned. If, on the other hand, the device name is specified but the device is no present in the xml configuration file, the device open function will throw an error and return false --- .../FrameTransformClient.cpp | 54 ++++++++++++------- 1 file changed, 35 insertions(+), 19 deletions(-) diff --git a/src/devices/frameTransformClient/FrameTransformClient.cpp b/src/devices/frameTransformClient/FrameTransformClient.cpp index 779513aac78..6f611cefad6 100644 --- a/src/devices/frameTransformClient/FrameTransformClient.cpp +++ b/src/devices/frameTransformClient/FrameTransformClient.cpp @@ -352,36 +352,52 @@ bool FrameTransformClient::open(yarp::os::Searchable &config) return false; } - std::string setdeviceName = "ftc_storage"; - if (m_robot.hasParam("setDeviceName")) { setdeviceName = m_robot.findParam("setDeviceName");} - if (!m_robot.hasDevice(setdeviceName)) + std::string setdeviceName; + if (m_robot.hasParam("setDeviceName")) { - yCWarning(FRAMETRANSFORMCLIENT) << "Set device name was not provided in the specified configuration. Set operations will not be available"; - } - else - { - auto* polyset = m_robot.device(setdeviceName).driver(); - if (!polyset || !polyset->view(m_ift_set)) + setdeviceName = m_robot.findParam("setDeviceName"); + if (!m_robot.hasDevice(setdeviceName)) { - yCError(FRAMETRANSFORMCLIENT) << "Failed to open device driver / interface for " << setdeviceName; + yCError(FRAMETRANSFORMCLIENT) << "Set device specified (" << setdeviceName << ") was not found in the configuration. The configuration is worngly writte, please check again"; return false; } + else + { + auto* polyset = m_robot.device(setdeviceName).driver(); + if (!polyset || !polyset->view(m_ift_set)) + { + yCError(FRAMETRANSFORMCLIENT) << "Failed to open device driver / interface for " << setdeviceName; + return false; + } + } } - - std::string getdeviceName = "ftc_storage"; - if (m_robot.hasParam("getDeviceName")) {getdeviceName = m_robot.findParam("getDeviceName");} - if (!m_robot.hasDevice(getdeviceName)) + else { - yCWarning(FRAMETRANSFORMCLIENT) << "Get device name was not provided in the specified configuration. Get operations will not be available"; + yCWarning(FRAMETRANSFORMCLIENT) << "Set device name was not provided in the specified configuration. Set operations will not be available"; } - else + + std::string getdeviceName; + if (m_robot.hasParam("getDeviceName")) { - auto* polyget = m_robot.device(getdeviceName).driver(); - if (!polyget || !polyget->view(m_ift_get) || !polyget->view(m_ift_util)) + getdeviceName = m_robot.findParam("getDeviceName"); + if (!m_robot.hasDevice(getdeviceName)) { - yCError(FRAMETRANSFORMCLIENT) << "Failed to open device driver / interface for " << getdeviceName; + yCError(FRAMETRANSFORMCLIENT) << "Get device specified (" << getdeviceName << ") was not found in the configuration. The configuration is worngly writte, please check again"; return false; } + else + { + auto* polyget = m_robot.device(getdeviceName).driver(); + if (!polyget || !polyget->view(m_ift_get) || !polyget->view(m_ift_util)) + { + yCError(FRAMETRANSFORMCLIENT) << "Failed to open device driver / interface for " << getdeviceName; + return false; + } + } + } + else + { + yCWarning(FRAMETRANSFORMCLIENT) << "Get device name was not provided in the specified configuration. Get operations will not be available"; } if (config.check("period")) From d3fcc3c7ea6f4a26c5201ae9faec0b7288af61b2 Mon Sep 17 00:00:00 2001 From: elandini84 <62991576+elandini84@users.noreply.github.com> Date: Fri, 20 Jan 2023 12:08:59 +0100 Subject: [PATCH 5/6] Update src/devices/frameTransformClient/FrameTransformClient.cpp Co-authored-by: Marco Randazzo --- src/devices/frameTransformClient/FrameTransformClient.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/devices/frameTransformClient/FrameTransformClient.cpp b/src/devices/frameTransformClient/FrameTransformClient.cpp index 6f611cefad6..b2560fb9406 100644 --- a/src/devices/frameTransformClient/FrameTransformClient.cpp +++ b/src/devices/frameTransformClient/FrameTransformClient.cpp @@ -358,7 +358,7 @@ bool FrameTransformClient::open(yarp::os::Searchable &config) setdeviceName = m_robot.findParam("setDeviceName"); if (!m_robot.hasDevice(setdeviceName)) { - yCError(FRAMETRANSFORMCLIENT) << "Set device specified (" << setdeviceName << ") was not found in the configuration. The configuration is worngly writte, please check again"; + yCError(FRAMETRANSFORMCLIENT) << "Set device specified (" << setdeviceName << ") was not found in the configuration. The configuration is wrongly written, please check again"; return false; } else From 10b73ca4c61668a6f667af5615fd1a937c6fb2cf Mon Sep 17 00:00:00 2001 From: elandini84 <62991576+elandini84@users.noreply.github.com> Date: Fri, 20 Jan 2023 12:09:12 +0100 Subject: [PATCH 6/6] Update src/devices/frameTransformClient/FrameTransformClient.cpp Co-authored-by: Marco Randazzo --- src/devices/frameTransformClient/FrameTransformClient.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/devices/frameTransformClient/FrameTransformClient.cpp b/src/devices/frameTransformClient/FrameTransformClient.cpp index b2560fb9406..4d247845d03 100644 --- a/src/devices/frameTransformClient/FrameTransformClient.cpp +++ b/src/devices/frameTransformClient/FrameTransformClient.cpp @@ -382,7 +382,7 @@ bool FrameTransformClient::open(yarp::os::Searchable &config) getdeviceName = m_robot.findParam("getDeviceName"); if (!m_robot.hasDevice(getdeviceName)) { - yCError(FRAMETRANSFORMCLIENT) << "Get device specified (" << getdeviceName << ") was not found in the configuration. The configuration is worngly writte, please check again"; + yCError(FRAMETRANSFORMCLIENT) << "Get device specified (" << getdeviceName << ") was not found in the configuration. The configuration is wrongly written, please check again"; return false; } else