Skip to content

Implement configurable frames for virtual objects #109

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
erblinium wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Implement configurable frames for virtual objects #109

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cb31082
Implement configurable frames for virtual objects
erblinium Feb 8, 2023
bfae7e2
WIP Refactor updateObjectLinks()
erblinium Feb 24, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions ensenso_camera/include/ensenso_camera/virtual_object_handler.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ class VirtualObjectHandler

/// Original object transforms in the base frame
std::vector<tf2::Transform> originalTransforms;
std::vector<std::string> objectFrames;
Copy link
Contributor

@benthie benthie Feb 23, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are the object frames going to change during operation? I guess not, and if so, I think we do not need to store them but can read them from the NxLib tree, see the comments in the .cpp file.


std::string objectsFrame; ///< Frame in which objects are defined
std::string linkFrame; ///< Link frame where NxLib expects the objects to be defined
Expand Down
41 changes: 21 additions & 20 deletions ensenso_camera/src/virtual_object_handler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_)
Copy link
Contributor

@benthie benthie Feb 23, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

frame isn't used anymore

Copy link
Contributor

@benthie benthie Feb 23, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As mentioned below, objects are not needed to be passed in but can be read from the tree:
auto objects = NxLibItem[itmObjects];

Copy link
Contributor Author

@erblinium erblinium Feb 24, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Getting the following error when using the suggested updateObjectLinks() function:
src/ros_driver/ensenso_camera2/src/virtual_object_handler.cpp:205:27: error: expected primary-expression before ‘[’ token 205 | auto objects = NxLibItem[itmObjects];

Copy link
Contributor

@benthie benthie Feb 24, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry, that must be NxLibItem()[itmObjects];

Copy link
Contributor Author

@erblinium erblinium Feb 24, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Got the following runtime error now after using NxLibItem()[itmObjects];:
NxLibException 13 (ItemTypeNotCompatible) for item /Objects/\0/Link/Target

Copy link
Contributor

@benthie benthie Feb 24, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If it's okay for you, push your temporary changes and then I can have a look ;)

Copy link
Contributor Author

@erblinium erblinium Feb 24, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Line 213

Copy link
Contributor Author

@erblinium erblinium Feb 24, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Which version of Ensenso SDK do you use?
I am using 3.3.1417

Copy link
Contributor

@benthie benthie Feb 24, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I just tried it with both 3.3.1417 and 3.4.743, both with the same results.

In line 168 you write the objects to the NxLib tree. From there on they are in the tree. Getting the target name in line 213 should work, unless you are modifying the tree from somewhere else? Are you perhaps adding or removing objects in the meantime? The error could e.g. occur if you added another object meanwhile, which has no target node.

In case you want to be able to add and remove objects during operation, storing the object frames as you initially did would be required.

Copy link
Contributor Author

@erblinium erblinium Feb 27, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes I do add other objects in the meantime. I have two ensenso_camera_node nodes each with their own virtual objects file. They are both started from the same launch file.

Copy link
Contributor

@benthie benthie Feb 27, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay I see. Then there is the problem.

Since you guys are currently the only one developing (and maybe even using) this feature, it is quite hard for me - or us in general - to review the code. Do you think you could maybe write a test case that also acts as kind of a documentation? Then I can get an idea of how your code is supposed to work.

: nh(std::move(_nh)), rate(publishRate), stop(stop_)
{
{
// Create output topic
publisher = ensenso::ros::create_publisher<visualization_msgs::msg::MarkerArray>(nh, topic, 1);

Expand All @@ -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
Expand Down Expand Up @@ -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());
Copy link
Contributor

@benthie benthie Feb 23, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If we do not need to store the objects frames, this line can be removed.

Copy link
Contributor

@benthie benthie Feb 23, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Lines 166 and 167 could be changed to the following in order to make the interaction with the tree more clear:

  // Read the file content and write it to the objects node in the tree
  auto objects = NxLibItem[itmObjects];

Since the objects are now written to the tree, we can simply read them from the tree whenever we need and do not have to pass the objects around. Both objects and objectsFrame then do not have to be passed to the VirtualObjectMarkerPublisher constructor.

}

// Create publisher thread
Expand Down Expand Up @@ -201,24 +202,24 @@ 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;
}

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)
{
tf2::Transform objectTransform = cameraTransform * originalTransforms[i];
NxLibItem objectLink = NxLibItem{ itmObjects }[static_cast<int>(i)][itmLink];
writeTransformToNxLib(objectTransform.inverse(), objectLink);
auto objectLink = objects[i][itmLink];
try
{
// 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;
}
}
}
}