LocalGuidance
A library that generates velocity references to follow a path.
navigation_common::PoseStamped Class Reference

#include <PoseStamped.hpp>

Public Types

typedef std_msgs::Header Header
 

Public Member Functions

 PoseStamped ()=default
 
 PoseStamped (const Header &header, const Vector3D &position, const RotationQuaternionD &orientation=RotationQuaternionD(), bool ignoreOrientation=false, const Tolerance &tolerance=Tolerance(), const std::string &comment="")
 
 PoseStamped (const navigation_msgs::PoseStamped &poseStamped)
 
 PoseStamped (const geometry_msgs::PoseStamped &poseStamped, bool ignoreOrientation=false, const Tolerance &tolerance=Tolerance(), const std::string &comment="")
 
 ~PoseStamped ()=default
 
void fromRos (const navigation_msgs::PoseStamped &msg)
 
navigation_msgs::PoseStamped toRos () const
 
void setRosPoseStamped (const geometry_msgs::PoseStamped &msg)
 
void fromRosPoseStamped (const geometry_msgs::PoseStamped &msg, bool ignoreOrientation=false, const Tolerance &tolerance=Tolerance(), const std::string &comment="")
 
geometry_msgs::PoseStamped toRosPoseStamped () const
 
geometry_msgs::Pose toRosPose () const
 
bool fromString (const std::string &string)
 
std::string toString () const
 
void fromFile (const std::string &absolutePath)
 
void toFile (const std::string &absolutePath) const
 
PoseStamped inverse () const
 
PoseStampedinvert ()
 
bool isNear (const PoseStamped &other, double positionTol, double orientationTol) const
 
double getYaw () const
 
void addYawToZ (double zScale, double yawScale)
 
std::string getComment () const
 
void setComment (const std::string &comment)
 

Public Attributes

Header header_
 Header. More...
 
Vector3D position_ = Vector3D::Zero()
 Position. More...
 
RotationQuaternionD orientation_
 Orientation (is initialized with identity). More...
 
bool ignoreOrientation_ = false
 Ignore orientation. More...
 
Tolerance tolerance_
 Tolerance. More...
 

Static Protected Member Functions

static geometry_msgs::Point toRos (const Vector3D &position)
 
static geometry_msgs::Quaternion toRos (const RotationQuaternionD &orientation)
 
static Vector3D fromRos (const geometry_msgs::Point &positionMsg)
 
static RotationQuaternionD fromRos (const geometry_msgs::Quaternion &orientationMsg)
 

Protected Attributes

std::string comment_
 Comment. More...
 

Member Typedef Documentation

◆ Header

typedef std_msgs::Header navigation_common::PoseStamped::Header

Constructor & Destructor Documentation

◆ PoseStamped() [1/4]

navigation_common::PoseStamped::PoseStamped ( )
default

Constructor.

◆ PoseStamped() [2/4]

navigation_common::PoseStamped::PoseStamped ( const Header header,
const Vector3D position,
const RotationQuaternionD orientation = RotationQuaternionD(),
bool  ignoreOrientation = false,
const Tolerance tolerance = Tolerance(),
const std::string &  comment = "" 
)
explicit

Constructor.

Parameters
headerheader.
positionposition.
orientationorientation.
ignoreOrientationignore orientation.

◆ PoseStamped() [3/4]

navigation_common::PoseStamped::PoseStamped ( const navigation_msgs::PoseStamped poseStamped)
explicit

Constructor.

Parameters
poseStampedros pose stamped.

◆ PoseStamped() [4/4]

navigation_common::PoseStamped::PoseStamped ( const geometry_msgs::PoseStamped &  poseStamped,
bool  ignoreOrientation = false,
const Tolerance tolerance = Tolerance(),
const std::string &  comment = "" 
)
explicit

Constructor.

Parameters
poseStampedros pose stamped.
ignoreOrientationignore orientation.

◆ ~PoseStamped()

navigation_common::PoseStamped::~PoseStamped ( )
default

Destructor.

Member Function Documentation

◆ addYawToZ()

void navigation_common::PoseStamped::addYawToZ ( double  zScale,
double  yawScale 
)

Add the yaw angle to z coordinate for visual debugging purposes. z := zScale*z + yawScale*yaw

Parameters
zScalescaling of z.
yawScalescaling of yaw.

◆ fromFile()

void navigation_common::PoseStamped::fromFile ( const std::string &  absolutePath)

Loads the pose from a file.

Parameters
absolutePathabsolute path to pose file.

◆ fromRos() [1/3]

void navigation_common::PoseStamped::fromRos ( const navigation_msgs::PoseStamped msg)

Imports a ros pose stamped message.

Parameters
msgros pose stamped message.

◆ fromRos() [2/3]

static Vector3D navigation_common::PoseStamped::fromRos ( const geometry_msgs::Point &  positionMsg)
staticprotected

◆ fromRos() [3/3]

static RotationQuaternionD navigation_common::PoseStamped::fromRos ( const geometry_msgs::Quaternion &  orientationMsg)
staticprotected

◆ fromRosPoseStamped()

void navigation_common::PoseStamped::fromRosPoseStamped ( const geometry_msgs::PoseStamped &  msg,
bool  ignoreOrientation = false,
const Tolerance tolerance = Tolerance(),
const std::string &  comment = "" 
)

Imports a ros pose stamped message.

Parameters
msgros pose stamped message.
ignoreOrientationignore orientation.
tolerancetolerance.

◆ fromString()

bool navigation_common::PoseStamped::fromString ( const std::string &  string)

Reads the pose from a string with the following format: frame_id stamp px py pz qw qx qy qz ignoreOrientation tolTrans tolRot comment

Parameters
stringstring containing the data.
Returns
true if successful.

◆ getComment()

std::string navigation_common::PoseStamped::getComment ( ) const

Get the comment.

Returns
Comment

◆ getYaw()

double navigation_common::PoseStamped::getYaw ( ) const

Get yaw of the rotation projected to the xy-plane.

Returns
yaw.

◆ inverse()

PoseStamped navigation_common::PoseStamped::inverse ( ) const

Gets the pose with inverted orientation.

Returns
inverted pose.

◆ invert()

PoseStamped& navigation_common::PoseStamped::invert ( )

Inverts the orientation of the pose.

Returns
reference to this object.

◆ isNear()

bool navigation_common::PoseStamped::isNear ( const PoseStamped other,
double  positionTol,
double  orientationTol 
) const

Comparison function.

Parameters
otherother pose.
positionTolnumerical tolerance for the position error.
orientationTolnumerical tolerance for the orientation error.
Returns
true if poses are near.

◆ setComment()

void navigation_common::PoseStamped::setComment ( const std::string &  comment)

Set the comment.

Parameters
commentComment

◆ setRosPoseStamped()

void navigation_common::PoseStamped::setRosPoseStamped ( const geometry_msgs::PoseStamped &  msg)

Imports a ros pose stamped message without overwriting the other members.

Parameters
msgros pose stamped message.

◆ toFile()

void navigation_common::PoseStamped::toFile ( const std::string &  absolutePath) const

Saves the pose to a file.

Parameters
absolutePathabsolute path to pose file.

◆ toRos() [1/3]

navigation_msgs::PoseStamped navigation_common::PoseStamped::toRos ( ) const

Exports a ros pose stamped message.

Returns
ros pose stamped message.

◆ toRos() [2/3]

static geometry_msgs::Point navigation_common::PoseStamped::toRos ( const Vector3D position)
staticprotected

◆ toRos() [3/3]

static geometry_msgs::Quaternion navigation_common::PoseStamped::toRos ( const RotationQuaternionD orientation)
staticprotected

◆ toRosPose()

geometry_msgs::Pose navigation_common::PoseStamped::toRosPose ( ) const

Exports a ros pose message.

Returns
ros pose message.

◆ toRosPoseStamped()

geometry_msgs::PoseStamped navigation_common::PoseStamped::toRosPoseStamped ( ) const

Exports a ros pose stamped message.

Returns
ros pose stamped message.

◆ toString()

std::string navigation_common::PoseStamped::toString ( ) const

Writes the pose to a string with the following format: frame_id stamp px py pz qw qx qy qz ignoreOrientation tolTrans tolRot comment

Member Data Documentation

◆ comment_

std::string navigation_common::PoseStamped::comment_
protected

Comment.

◆ header_

Header navigation_common::PoseStamped::header_

Header.

◆ ignoreOrientation_

bool navigation_common::PoseStamped::ignoreOrientation_ = false

Ignore orientation.

◆ orientation_

RotationQuaternionD navigation_common::PoseStamped::orientation_

Orientation (is initialized with identity).

◆ position_

Vector3D navigation_common::PoseStamped::position_ = Vector3D::Zero()

Position.

◆ tolerance_

Tolerance navigation_common::PoseStamped::tolerance_

The documentation for this class was generated from the following file: