From cb31082cf22655fa6bc313c5d2f203a3c63b350e Mon Sep 17 00:00:00 2001 From: erblinium Date: Wed, 8 Feb 2023 09:35:38 +0100 Subject: [PATCH 1/2] Implement configurable frames for virtual objects --- .../ensenso_camera/virtual_object_handler.h | 1 + ensenso_camera/src/virtual_object_handler.cpp | 31 ++++++++++--------- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/ensenso_camera/include/ensenso_camera/virtual_object_handler.h b/ensenso_camera/include/ensenso_camera/virtual_object_handler.h index 65a1a021..8ca86f28 100644 --- a/ensenso_camera/include/ensenso_camera/virtual_object_handler.h +++ b/ensenso_camera/include/ensenso_camera/virtual_object_handler.h @@ -27,6 +27,7 @@ class VirtualObjectHandler /// Original object transforms in the base frame std::vector originalTransforms; + std::vector objectFrames; std::string objectsFrame; ///< Frame in which objects are defined std::string linkFrame; ///< Link frame where NxLib expects the objects to be defined diff --git a/ensenso_camera/src/virtual_object_handler.cpp b/ensenso_camera/src/virtual_object_handler.cpp index a2ff5b81..404ab630 100644 --- a/ensenso_camera/src/virtual_object_handler.cpp +++ b/ensenso_camera/src/virtual_object_handler.cpp @@ -41,7 +41,7 @@ struct VirtualObjectMarkerPublisher VirtualObjectMarkerPublisher(ensenso::ros::NodeHandle _nh, const std::string& topic, double publishRate, const NxLibItem& objects, const std::string& frame, std::atomic_bool& stop_) : nh(std::move(_nh)), rate(publishRate), stop(stop_) - { + { // Create output topic publisher = ensenso::ros::create_publisher(nh, topic, 1); @@ -55,7 +55,7 @@ struct VirtualObjectMarkerPublisher marker.ns = ensenso::ros::get_node_name(nh); marker.id = i; marker.header.stamp = ensenso::ros::Time(0); - marker.header.frame_id = frame; + marker.header.frame_id = object[itmLink][itmTarget].asString(); marker.action = visualization_msgs::msg::Marker::MODIFY; // Note: ADD = MODIFY // Set color @@ -171,6 +171,7 @@ VirtualObjectHandler::VirtualObjectHandler(ensenso::ros::NodeHandle& nh, const s for (int i = 0; i < objects.count(); ++i) { originalTransforms.push_back(transformFromNxLib(objects[i][itmLink])); + objectFrames.push_back(objects[i][itmLink][itmTarget].asString()); } // Create publisher thread @@ -201,24 +202,26 @@ void VirtualObjectHandler::updateObjectLinks() { return; } - - // Find transform from the frame in which the objects were defined to the current optical frame + tf2::Transform cameraTransform; - try - { - cameraTransform = fromMsg(tfBuffer.lookupTransform(linkFrame, objectsFrame, ensenso::ros::Time(0)).transform); - } - catch (const tf2::TransformException& e) - { - ENSENSO_WARN("Could not look up the virtual object pose due to the TF error: %s", e.what()); - return; - } // Apply the transform to all of the original transforms for (size_t i = 0; i < originalTransforms.size(); ++i) { + // Find transform from the frame in which the objects were defined to the current optical frame + try + { + cameraTransform = fromMsg(tfBuffer.lookupTransform(linkFrame, objectFrames[i], ensenso::ros::Time(0)).transform); + } + catch (const tf2::TransformException& e) + { + ENSENSO_WARN("Could not look up the virtual object pose due to the TF error: %s", e.what()); + return; + } + + // Transform object back to original frame tf2::Transform objectTransform = cameraTransform * originalTransforms[i]; NxLibItem objectLink = NxLibItem{ itmObjects }[static_cast(i)][itmLink]; writeTransformToNxLib(objectTransform.inverse(), objectLink); } -} +} \ No newline at end of file From bfae7e23640acc5aabfb9d5739517cb71b215851 Mon Sep 17 00:00:00 2001 From: erblinium Date: Fri, 24 Feb 2023 10:52:30 +0100 Subject: [PATCH 2/2] WIP Refactor updateObjectLinks() --- ensenso_camera/src/virtual_object_handler.cpp | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/ensenso_camera/src/virtual_object_handler.cpp b/ensenso_camera/src/virtual_object_handler.cpp index 404ab630..37e6d205 100644 --- a/ensenso_camera/src/virtual_object_handler.cpp +++ b/ensenso_camera/src/virtual_object_handler.cpp @@ -202,26 +202,24 @@ void VirtualObjectHandler::updateObjectLinks() { return; } - - tf2::Transform cameraTransform; - + auto objects = NxLibItem()[itmObjects]; // Apply the transform to all of the original transforms - for (size_t i = 0; i < originalTransforms.size(); ++i) + for (int i = 0; i < objects.count(); ++i) { - // Find transform from the frame in which the objects were defined to the current optical frame + auto objectLink = objects[i][itmLink]; try { - cameraTransform = fromMsg(tfBuffer.lookupTransform(linkFrame, objectFrames[i], ensenso::ros::Time(0)).transform); + // Find the transformation from the object frame to the current optical frame + auto objectFrame = objectLink[itmTarget].asString(); + auto cameraTransform = fromMsg(tfBuffer.lookupTransform(linkFrame, objectFrame, ensenso::ros::Time(0)).transform); + // Transform object back to original frame + tf2::Transform objectTransform = cameraTransform * originalTransforms[i]; + writeTransformToNxLib(objectTransform.inverse(), objectLink); } catch (const tf2::TransformException& e) { ENSENSO_WARN("Could not look up the virtual object pose due to the TF error: %s", e.what()); return; } - - // Transform object back to original frame - tf2::Transform objectTransform = cameraTransform * originalTransforms[i]; - NxLibItem objectLink = NxLibItem{ itmObjects }[static_cast(i)][itmLink]; - writeTransformToNxLib(objectTransform.inverse(), objectLink); } -} \ No newline at end of file +} \ No newline at end of file