pose_utilities.h

Go to the documentation of this file.

#pragma once

#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> // Needed for conversion from geometry_msgs to tf2::Transform
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <string>
#include <tf2_ros/buffer.h>
#include <tf2/LinearMath/Transform.h>

#include "nxLib.h"

bool isValid(tf2::Transform const& pose);
bool isValid(geometry_msgs::Transform const& pose);
bool isValid(tf2::Vector3 const& vector);

void writePoseToNxLib(tf2::Transform const& pose, NxLibItem const& node);

tf2::Transform poseFromNxLib(NxLibItem const& node);

geometry_msgs::TransformStamped poseFromNxLib(NxLibItem const& node, std::string const& parentFrame,
                                              std::string const& childFrame);

geometry_msgs::TransformStamped transformFromPose(geometry_msgs::PoseStamped const& pose,
                                                  std::string const& childFrame);

geometry_msgs::PoseStamped stampedPoseFromTransform(geometry_msgs::TransformStamped const& transform);
geometry_msgs::Pose poseFromTransform(tf2::Transform const& transform);
geometry_msgs::TransformStamped fromTfTransform(tf2::Transform const& transform, std::string parentFrame,
                                                std::string childFrame);

tf2::Transform fromMsg(geometry_msgs::Transform const& transform);
tf2::Transform fromMsg(geometry_msgs::Pose const& pose);
tf2::Transform fromStampedMessage(geometry_msgs::TransformStamped const& transform);
tf2::Transform fromStampedMessage(geometry_msgs::PoseStamped const& pose);

tf2::Transform getLatestTransform(tf2_ros::Buffer const& tfBuffer, std::string const& cameraFrame,
                                  std::string const& targetFrame);