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