17 std::string readFile(
const std::string &filename)
19 std::ifstream file{ filename };
20 if (!file.is_open() || !file.rdbuf()) {
21 throw std::runtime_error(
"Unable to read objects file");
24 std::stringstream buffer;
25 buffer << file.rdbuf();
31 objectsFrame(objectsFrame),
32 cameraFrame(cameraFrame)
34 nxLibInitialize(
false);
37 auto objects = NxLibItem{ itmObjects };
38 objects.setJson(readFile(filename));
41 for (
int i = 0; i < objects.count(); ++i) {
61 ROS_WARN(
"Could not look up the virtual object pose due to the TF error: %s", e.what());
69 NxLibItem objectLink = NxLibItem{ itmObjects }[
static_cast<int>(i)][itmLink];
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
tf2::Transform fromMsg(geometry_msgs::Transform const &transform)
void writePoseToNxLib(tf2::Transform const &pose, NxLibItem const &node)
std::string objectsFrame
Frame in which objects are defined.
VirtualObjectHandler(const std::string &filename, const std::string &objectsFrame, const std::string &cameraFrame)
std::string cameraFrame
Optical frame of the camera.
std::vector< tf2::Transform > originalPoses
Original object poses in the base frame.
tf2::Transform poseFromNxLib(NxLibItem const &node)