From ff300ae9ef1594e7de560ee776312af98acab454 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 0f82f09d46bae88d15a829ca07a8f5804ee6fa7e Mon Sep 17 00:00:00 2001 From: Erblin Ujkani Date: Fri, 9 Jun 2023 17:01:26 +0200 Subject: [PATCH 2/2] Add ensenso and halcon calibration plates to virtual object handler --- ensenso_camera/src/virtual_object_handler.cpp | 372 +++++++++--------- 1 file changed, 193 insertions(+), 179 deletions(-) diff --git a/ensenso_camera/src/virtual_object_handler.cpp b/ensenso_camera/src/virtual_object_handler.cpp index 404ab630..988fd540 100644 --- a/ensenso_camera/src/virtual_object_handler.cpp +++ b/ensenso_camera/src/virtual_object_handler.cpp @@ -22,206 +22,220 @@ using namespace ensenso_camera; namespace { -std::string readFile(const std::string& filename) -{ - std::ifstream file{ filename }; - if (!file.is_open() || !file.rdbuf()) - { - throw std::runtime_error("Unable to read objects file"); - } - - std::stringstream buffer; - buffer << file.rdbuf(); - return buffer.str(); -} - -/// Class responsible for publishing NxLib virtual objects as ROS visualization_msgs::MarkerArray -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); - - // Collect markers - for (int i = 0; i < objects.count(); ++i) + std::string readFile(const std::string& filename) { - auto& object = objects[i]; - - visualization_msgs::msg::Marker marker; - - marker.ns = ensenso::ros::get_node_name(nh); - marker.id = i; - marker.header.stamp = ensenso::ros::Time(0); - marker.header.frame_id = object[itmLink][itmTarget].asString(); - marker.action = visualization_msgs::msg::Marker::MODIFY; // Note: ADD = MODIFY - - // Set color - const auto color = object[itmLighting][itmColor]; - if (color.exists() && color.count() == 3) - { - marker.color.r = static_cast(color[0].asDouble()); - marker.color.g = static_cast(color[1].asDouble()); - marker.color.b = static_cast(color[2].asDouble()); - marker.color.a = 1.0f; - } - else - { - // Default color (gray) - marker.color.r = 0.5; - marker.color.g = 0.5; - marker.color.b = 0.5; - marker.color.a = 1.0; - } - - // Set pose - marker.pose = poseFromTransform(transformFromNxLib(object[itmLink])); - - auto type = object[itmType]; - auto filename = object[itmFilename]; - - // Set type and deal with object-specific properties - if (filename.exists()) - { - marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; - marker.mesh_resource = "file://" + filename.asString(); - - // Scale from mm to m - marker.scale.x = 1.0 / 1000.0; - marker.scale.y = 1.0 / 1000.0; - marker.scale.z = 1.0 / 1000.0; - } - else if (type.asString() == valSphere) - { - marker.type = visualization_msgs::msg::Marker::SPHERE; - - // Sphere scale is diameter in meters - marker.scale.x = object[itmRadius].asDouble() * 2.0 / 1000.0; - marker.scale.y = marker.scale.x; - marker.scale.z = marker.scale.x; - } - else if (type.asString() == valCylinder) - { - marker.type = visualization_msgs::msg::Marker::CYLINDER; - - /// Cylinder scale x/y is diameter in meters, z is height in meters. - marker.scale.x = object[itmRadius].asDouble() * 2.0 / 1000.0; - marker.scale.y = marker.scale.x; - marker.scale.z = object[itmHeight].asDouble() / 1000.0; - - /// Cylinders in NxLib start at the base. - /// Cylinders in ROS start at the centroid. - // marker.pose.position.z += marker.scale.z / 2.0; - } - else if (type.asString() == valCube) - { - marker.type = visualization_msgs::msg::Marker::CUBE; - - marker.scale.x = object[itmWidth].asDouble() / 1000.0; - marker.scale.y = object[itmWidth].asDouble() / 1000.0; - marker.scale.z = object[itmWidth].asDouble() / 1000.0; - } - else if (type.asString() == valCuboid) - { - marker.type = visualization_msgs::msg::Marker::CUBE; - - marker.scale.x = object[itmWidth].asDouble() / 1000.0; - marker.scale.z = object[itmHeight].asDouble() / 1000.0; - marker.scale.y = object[itmDepth].asDouble() / 1000.0; - } - else - { - // Unsupported type, skip it - continue; - } - - markers.markers.push_back(marker); + std::ifstream file{ filename }; + if (!file.is_open() || !file.rdbuf()) + { + throw std::runtime_error("Unable to read objects file"); + } + + std::stringstream buffer; + buffer << file.rdbuf(); + return buffer.str(); } - } - void run() - { - while (ensenso::ros::ok() && !stop) +/// Class responsible for publishing NxLib virtual objects as ROS visualization_msgs::MarkerArray + struct VirtualObjectMarkerPublisher { - publisher->publish(markers); - rate.sleep(); - } - } - - ensenso::ros::NodeHandle nh; - ensenso::ros::Publisher publisher; - ensenso::ros::Rate rate; - visualization_msgs::msg::MarkerArray markers; - std::atomic_bool& stop; -}; + 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); + + // Collect markers + for (int i = 0; i < objects.count(); ++i) + { + auto& object = objects[i]; + + visualization_msgs::msg::Marker marker; + + marker.ns = ensenso::ros::get_node_name(nh); + marker.id = i; + marker.header.stamp = ensenso::ros::Time(0); + marker.header.frame_id = object[itmLink][itmTarget].asString(); + marker.action = visualization_msgs::msg::Marker::MODIFY; // Note: ADD = MODIFY + + // Set color + const auto color = object[itmLighting][itmColor]; + if (color.exists() && color.count() == 3) + { + marker.color.r = static_cast(color[0].asDouble()); + marker.color.g = static_cast(color[1].asDouble()); + marker.color.b = static_cast(color[2].asDouble()); + marker.color.a = 1.0f; + } + else + { + // Default color (gray) + marker.color.r = 0.5; + marker.color.g = 0.5; + marker.color.b = 0.5; + marker.color.a = 1.0; + } + + // Set pose + marker.pose = poseFromTransform(transformFromNxLib(object[itmLink])); + + auto type = object[itmType]; + auto filename = object[itmFilename]; + + // Set type and deal with object-specific properties + if (filename.exists()) + { + marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; + marker.mesh_resource = "file://" + filename.asString(); + + // Scale from mm to m + marker.scale.x = 1.0 / 1000.0; + marker.scale.y = 1.0 / 1000.0; + marker.scale.z = 1.0 / 1000.0; + } + else if (type.asString() == valSphere) + { + marker.type = visualization_msgs::msg::Marker::SPHERE; + + // Sphere scale is diameter in meters + marker.scale.x = object[itmRadius].asDouble() * 2.0 / 1000.0; + marker.scale.y = marker.scale.x; + marker.scale.z = marker.scale.x; + } + else if (type.asString() == valCylinder) + { + marker.type = visualization_msgs::msg::Marker::CYLINDER; + + /// Cylinder scale x/y is diameter in meters, z is height in meters. + marker.scale.x = object[itmRadius].asDouble() * 2.0 / 1000.0; + marker.scale.y = marker.scale.x; + marker.scale.z = object[itmHeight].asDouble() / 1000.0; + + /// Cylinders in NxLib start at the base. + /// Cylinders in ROS start at the centroid. + // marker.pose.position.z += marker.scale.z / 2.0; + } + else if (type.asString() == valCube) + { + marker.type = visualization_msgs::msg::Marker::CUBE; + + marker.scale.x = object[itmWidth].asDouble() / 1000.0; + marker.scale.y = object[itmWidth].asDouble() / 1000.0; + marker.scale.z = object[itmWidth].asDouble() / 1000.0; + } + else if (type.asString() == valCuboid) + { + marker.type = visualization_msgs::msg::Marker::CUBE; + + marker.scale.x = object[itmWidth].asDouble() / 1000.0; + marker.scale.z = object[itmHeight].asDouble() / 1000.0; + marker.scale.y = object[itmDepth].asDouble() / 1000.0; + } + else if (type.asString() == valSingleCustom || type.asString() == valSingle) { + // Ensenso and halcon calibration type markers + marker.type = visualization_msgs::msg::Marker::CUBE; + auto grid_size_x = object[itmGridSize][0].asDouble(); + auto grid_size_y = object[itmGridSize][1].asDouble(); + auto grid_spacing = object[itmGridSpacing].asDouble(); + + auto plate_width = std::round(0.5 * grid_size_x * grid_spacing / 0.9); + auto plate_height = std::round(0.5 * grid_size_y * grid_spacing / 0.9); + + marker.scale.x = plate_width / 1000.0; + marker.scale.y = plate_height / 1000.0; + marker.scale.z = 3 / 1000.0; + } + else + { + // Unsupported type, skip it + continue; + } + + markers.markers.push_back(marker); + } + } + + void run() + { + while (ensenso::ros::ok() && !stop) + { + publisher->publish(markers); + rate.sleep(); + } + } + + ensenso::ros::NodeHandle nh; + ensenso::ros::Publisher publisher; + ensenso::ros::Rate rate; + visualization_msgs::msg::MarkerArray markers; + std::atomic_bool& stop; + }; } // namespace VirtualObjectHandler::VirtualObjectHandler(ensenso::ros::NodeHandle& nh, const std::string& filename, const std::string& objectsFrame, const std::string& linkFrame, const std::string& markerTopic, double markerPublishRate) - : objectsFrame(objectsFrame), linkFrame(linkFrame), tfBuffer(TF2_BUFFER_CTOR_ARGS(nh)) + : objectsFrame(objectsFrame), linkFrame(linkFrame), tfBuffer(TF2_BUFFER_CTOR_ARGS(nh)) { - // Read the file contents and assign it to the objects tag - auto objects = NxLibItem{ itmObjects }; - objects.setJson(readFile(filename)); - - // Get the original transforms from file - 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 - if (!markerTopic.empty()) - { - markerThread = std::thread([=]() { - VirtualObjectMarkerPublisher publisher{ nh, markerTopic, markerPublishRate, - objects, objectsFrame, stopMarkerThread }; - publisher.run(); - }); - } + // Read the file contents and assign it to the objects tag + auto objects = NxLibItem{ itmObjects }; + objects.setJson(readFile(filename)); + + // Get the original transforms from file + 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 + if (!markerTopic.empty()) + { + markerThread = std::thread([=]() { + VirtualObjectMarkerPublisher publisher{ nh, markerTopic, markerPublishRate, + objects, objectsFrame, stopMarkerThread }; + publisher.run(); + }); + } } VirtualObjectHandler::~VirtualObjectHandler() { - stopMarkerThread = true; + stopMarkerThread = true; - if (markerThread.joinable()) - { - markerThread.join(); - } + if (markerThread.joinable()) + { + markerThread.join(); + } } void VirtualObjectHandler::updateObjectLinks() { - // Exit early if we have no work to do - if (originalTransforms.empty()) - { - return; - } - - tf2::Transform cameraTransform; - - // 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 + // Exit early if we have no work to do + if (originalTransforms.empty()) { - cameraTransform = fromMsg(tfBuffer.lookupTransform(linkFrame, objectFrames[i], ensenso::ros::Time(0)).transform); + return; } - catch (const tf2::TransformException& e) + + tf2::Transform cameraTransform; + + // Apply the transform to all of the original transforms + for (size_t i = 0; i < originalTransforms.size(); ++i) { - ENSENSO_WARN("Could not look up the virtual object pose due to the TF error: %s", e.what()); - return; + // 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); } - - // 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