pose_utilities.h

Go to the documentation of this file.

#pragma once

#include "ensenso_camera/ros2/namespace.h"
#include "ensenso_camera/ros2/time.h"

#include "ensenso_camera/ros2/geometry_msgs/pose.h"
#include "ensenso_camera/ros2/geometry_msgs/pose_stamped.h"
#include "ensenso_camera/ros2/geometry_msgs/transform.h"
#include "ensenso_camera/ros2/geometry_msgs/transform_stamped.h"
#include "ensenso_camera/ros2/tf2_geometry_msgs/tf2_geometry_msgs.h"

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/buffer.h>

#include <string>

#include "nxLib.h"

using Transform = tf2::Transform;
using TransformMsg = geometry_msgs::msg::Transform;
using StampedTransformMsg = geometry_msgs::msg::TransformStamped;
using PoseMsg = geometry_msgs::msg::Pose;
using StampedPoseMsg = geometry_msgs::msg::PoseStamped;

bool isValid(Transform const& transform);
bool isValid(TransformMsg const& transform);

bool isIdentity(Transform const& transform);

void writeTransformToNxLib(Transform const& transform, NxLibItem const& node);

Transform transformFromNxLib(NxLibItem const& node);

StampedPoseMsg stampedPoseFromNxLib(NxLibItem const& node, std::string const& parentFrame,
                                    ensenso::ros::Time timestamp);

StampedTransformMsg transformFromPose(StampedPoseMsg const& pose, std::string const& childFrame);

StampedPoseMsg poseFromTransform(StampedTransformMsg const& transform);

PoseMsg poseFromTransform(Transform const& transform);

StampedTransformMsg fromTf(Transform const& transform, std::string parentFrame, std::string childFrame,
                           ensenso::ros::Time timestamp);

Transform fromMsg(TransformMsg const& transform);

Transform fromMsg(PoseMsg const& pose);

Transform fromMsg(StampedTransformMsg const& transform);

Transform fromMsg(StampedPoseMsg const& pose);

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

namespace tf2
{
void convertMsg(TransformMsg const& transform, PoseMsg& pose);
void convertMsg(PoseMsg const& pose, TransformMsg& transform);
void convertMsg(StampedTransformMsg const& transform, StampedPoseMsg& pose);
void convertMsg(StampedPoseMsg const& pose, StampedTransformMsg& transform);
}  // namespace tf2