This class implements a pose in the domain SE2: $\mathbb{R}^2 \times S^1$ The pose consist of the position x and y and an orientation given as angle theta [-pi, pi]. More...

#include <pose_se2.h>

Public Member Functions
	~PoseSE2 ()
	Destructs the PoseSE2. More...

Construct PoseSE2 instances
	PoseSE2 ()
	Default constructor. More...

	PoseSE2 (const Eigen::Ref< const Eigen::Vector2d > &position, double theta)
	Construct pose given a position vector and an angle theta. More...

	PoseSE2 (double x, double y, double theta)
	Construct pose using single components x, y, and the yaw angle. More...

	PoseSE2 (const geometry_msgs::Pose &pose)
	Construct pose using a geometry_msgs::Pose. More...

	PoseSE2 (const tf::Pose &pose)
	Construct pose using a tf::Pose. More...

	PoseSE2 (const PoseSE2 &pose)
	Copy constructor. More...

Access and modify values
Eigen::Vector2d &	position ()
	Access the 2D position part. More...

const Eigen::Vector2d &	position () const
	Access the 2D position part (read-only) More...

double &	x ()
	Access the x-coordinate the pose. More...

const double &	x () const
	Access the x-coordinate the pose (read-only) More...

double &	y ()
	Access the y-coordinate the pose. More...

const double &	y () const
	Access the y-coordinate the pose (read-only) More...

double &	theta ()
	Access the orientation part (yaw angle) of the pose. More...

const double &	theta () const
	Access the orientation part (yaw angle) of the pose (read-only) More...

void	setZero ()
	Set pose to [0,0,0]. More...

void	toPoseMsg (geometry_msgs::Pose &pose) const
	Convert PoseSE2 to a geometry_msgs::Pose. More...

Eigen::Vector2d	orientationUnitVec () const
	Return the unit vector of the current orientation. More...

Private Attributes
Eigen::Vector2d	_position

double	_theta

Arithmetic operations for which operators are not always reasonable
void	scale (double factor)
	Scale all SE2 components (x,y,theta) and normalize theta afterwards to [-pi, pi]. More...

void	plus (const double *pose_as_array)
	Increment the pose by adding a double[3] array The angle is normalized afterwards. More...

void	averageInPlace (const PoseSE2 &pose1, const PoseSE2 &pose2)
	Get the mean / average of two poses and store it in the caller class For the position part: 0.5*(x1+x2) For the angle: take the angle of the mean direction vector. More...

void	rotateGlobal (double angle, bool adjust_theta=true)
	Rotate pose globally. More...

static PoseSE2	average (const PoseSE2 &pose1, const PoseSE2 &pose2)
	Get the mean / average of two poses and return the result (static) For the position part: 0.5*(x1+x2) For the angle: take the angle of the mean direction vector. More...

Operator overloads / Allow some arithmetic operations
PoseSE2 &	operator= (const PoseSE2 &rhs)
	Asignment operator. More...

PoseSE2 &	operator+= (const PoseSE2 &rhs)
	Compound assignment operator (addition) More...

PoseSE2 &	operator-= (const PoseSE2 &rhs)
	Compound assignment operator (subtraction) More...

PoseSE2	operator+ (PoseSE2 lhs, const PoseSE2 &rhs)
	Arithmetic operator overload for additions. More...

PoseSE2	operator- (PoseSE2 lhs, const PoseSE2 &rhs)
	Arithmetic operator overload for subtractions. More...

PoseSE2	operator* (PoseSE2 pose, double scalar)
	Multiply pose with scalar and return copy without normalizing theta This operator is useful for calculating velocities ... More...

PoseSE2	operator* (double scalar, PoseSE2 pose)
	Multiply pose with scalar and return copy without normalizing theta This operator is useful for calculating velocities ... More...

std::ostream &	operator<< (std::ostream &stream, const PoseSE2 &pose)
	Output stream operator. More...

Detailed Description

This class implements a pose in the domain SE2: $\mathbb{R}^2 \times S^1$ The pose consist of the position x and y and an orientation given as angle theta [-pi, pi].

Definition at line 93 of file pose_se2.h.

Constructor & Destructor Documentation

◆ PoseSE2() [1/6]

teb_local_planner::PoseSE2::PoseSE2 ( )

inline

Default constructor.

Definition at line 103 of file pose_se2.h.

◆ PoseSE2() [2/6]

teb_local_planner::PoseSE2::PoseSE2	(	const Eigen::Ref< const Eigen::Vector2d > &	position,
		double	theta
	)

inline

Construct pose given a position vector and an angle theta.

Parameters

position	2D position vector
theta	angle given in rad

Definition at line 113 of file pose_se2.h.

◆ PoseSE2() [3/6]

teb_local_planner::PoseSE2::PoseSE2	(	double	x,
		double	y,
		double	theta
	)

inline

Construct pose using single components x, y, and the yaw angle.

Parameters

x	x-coordinate
y	y-coordinate
theta	yaw angle in rad

Definition at line 125 of file pose_se2.h.

◆ PoseSE2() [4/6]

teb_local_planner::PoseSE2::PoseSE2 ( const geometry_msgs::Pose & pose )

inline

Construct pose using a geometry_msgs::Pose.

Parameters

pose	geometry_msgs::Pose object

Definition at line 136 of file pose_se2.h.

◆ PoseSE2() [5/6]

teb_local_planner::PoseSE2::PoseSE2 ( const tf::Pose & pose )

inline

Construct pose using a tf::Pose.

Parameters

pose	tf::Pose object

Definition at line 147 of file pose_se2.h.

◆ PoseSE2() [6/6]

teb_local_planner::PoseSE2::PoseSE2 ( const PoseSE2 & pose )

inline

Copy constructor.

Parameters

pose	PoseSE2 instance

Definition at line 158 of file pose_se2.h.

◆ ~PoseSE2()

teb_local_planner::PoseSE2::~PoseSE2 ( )

inline

Destructs the PoseSE2.

Definition at line 170 of file pose_se2.h.

Member Function Documentation

◆ average()

static PoseSE2 teb_local_planner::PoseSE2::average	(	const PoseSE2 &	pose1,
		const PoseSE2 &	pose2
	)

inlinestatic

Get the mean / average of two poses and return the result (static) For the position part: 0.5*(x1+x2) For the angle: take the angle of the mean direction vector.

Parameters

pose1	first pose to consider
pose2	second pose to consider

Returns: mean / average of pose1 and pose2

Definition at line 302 of file pose_se2.h.

◆ averageInPlace()

void teb_local_planner::PoseSE2::averageInPlace	(	const PoseSE2 &	pose1,
		const PoseSE2 &	pose2
	)

inline

Get the mean / average of two poses and store it in the caller class For the position part: 0.5*(x1+x2) For the angle: take the angle of the mean direction vector.

Parameters

pose1	first pose to consider
pose2	second pose to consider

Definition at line 288 of file pose_se2.h.

◆ operator+=()

PoseSE2& teb_local_planner::PoseSE2::operator+= ( const PoseSE2 & rhs )

inline

Compound assignment operator (addition)

Parameters

rhs addend

Definition at line 350 of file pose_se2.h.

◆ operator-=()

PoseSE2& teb_local_planner::PoseSE2::operator-= ( const PoseSE2 & rhs )

inline

Compound assignment operator (subtraction)

Parameters

rhs	value to subtract

Definition at line 371 of file pose_se2.h.

◆ operator=()

PoseSE2& teb_local_planner::PoseSE2::operator= ( const PoseSE2 & rhs )

inline

Asignment operator.

Parameters

rhs	PoseSE2 instance

Todo:: exception safe version of the assignment operator

Definition at line 336 of file pose_se2.h.

◆ orientationUnitVec()

Eigen::Vector2d teb_local_planner::PoseSE2::orientationUnitVec ( ) const

inline

Return the unit vector of the current orientation.

Returns: [cos(theta), sin(theta))]^T

Definition at line 251 of file pose_se2.h.

◆ plus()

void teb_local_planner::PoseSE2::plus ( const double * pose_as_array )

inline

Increment the pose by adding a double[3] array The angle is normalized afterwards.

Parameters

pose_as_array 3D double array [x, y, theta]

Definition at line 274 of file pose_se2.h.

◆ position() [1/2]

Eigen::Vector2d& teb_local_planner::PoseSE2::position ( )

inline

Access the 2D position part.

See also: estimate

Returns: reference to the 2D position part

Definition at line 181 of file pose_se2.h.

◆ position() [2/2]

const Eigen::Vector2d& teb_local_planner::PoseSE2::position ( ) const

inline

Access the 2D position part (read-only)

See also: estimate

Returns: const reference to the 2D position part

Definition at line 188 of file pose_se2.h.

◆ rotateGlobal()

void teb_local_planner::PoseSE2::rotateGlobal	(	double	angle,
		bool	adjust_theta = `true`
	)

inline

Rotate pose globally.

Compute [pose_x, pose_y] = Rot(angle) * [pose_x, pose_y]. if adjust_theta, pose_theta is also rotated by angle

Parameters

angle	the angle defining the 2d rotation
adjust_theta	if `true`, the orientation theta is also rotated

Definition at line 315 of file pose_se2.h.

◆ scale()

void teb_local_planner::PoseSE2::scale ( double factor )

inline

Scale all SE2 components (x,y,theta) and normalize theta afterwards to [-pi, pi].

Parameters

factor scale factor

Definition at line 263 of file pose_se2.h.

◆ setZero()

void teb_local_planner::PoseSE2::setZero ( )

inline

Set pose to [0,0,0].

Definition at line 229 of file pose_se2.h.

◆ theta() [1/2]

double& teb_local_planner::PoseSE2::theta ( )

inline

Access the orientation part (yaw angle) of the pose.

Returns: reference to the yaw angle

Definition at line 218 of file pose_se2.h.

◆ theta() [2/2]

const double& teb_local_planner::PoseSE2::theta ( ) const

inline

Access the orientation part (yaw angle) of the pose (read-only)

Returns: const reference to the yaw angle

Definition at line 224 of file pose_se2.h.

◆ toPoseMsg()

void teb_local_planner::PoseSE2::toPoseMsg ( geometry_msgs::Pose & pose ) const

inline

Convert PoseSE2 to a geometry_msgs::Pose.

Parameters

[out] pose Pose message

Definition at line 239 of file pose_se2.h.

◆ x() [1/2]

double& teb_local_planner::PoseSE2::x ( )

inline

Access the x-coordinate the pose.

Returns: reference to the x-coordinate

Definition at line 194 of file pose_se2.h.

◆ x() [2/2]

const double& teb_local_planner::PoseSE2::x ( ) const

inline

Access the x-coordinate the pose (read-only)

Returns: const reference to the x-coordinate

Definition at line 200 of file pose_se2.h.

◆ y() [1/2]

double& teb_local_planner::PoseSE2::y ( )

inline

Access the y-coordinate the pose.

Returns: reference to the y-coordinate

Definition at line 206 of file pose_se2.h.

◆ y() [2/2]

const double& teb_local_planner::PoseSE2::y ( ) const

inline

Access the y-coordinate the pose (read-only)

Returns: const reference to the y-coordinate

Definition at line 212 of file pose_se2.h.

Friends And Related Function Documentation

◆ operator* [1/2]

PoseSE2 operator*	(	double	scalar,
		PoseSE2	pose
	)

friend

Multiply pose with scalar and return copy without normalizing theta This operator is useful for calculating velocities ...

Parameters

scalar	factor to multiply with
pose	pose to scale

Warning: theta is not normalized after multiplying

Definition at line 409 of file pose_se2.h.

◆ operator* [2/2]

PoseSE2 operator*	(	PoseSE2	pose,
		double	scalar
	)

friend

Multiply pose with scalar and return copy without normalizing theta This operator is useful for calculating velocities ...

Parameters

pose	pose to scale
scalar	factor to multiply with

Warning: theta is not normalized after multiplying

Definition at line 395 of file pose_se2.h.

◆ operator+

PoseSE2 operator+	(	PoseSE2	lhs,
		const PoseSE2 &	rhs
	)

friend

Arithmetic operator overload for additions.

Parameters

lhs	First addend
rhs	Second addend

Definition at line 362 of file pose_se2.h.

◆ operator-

PoseSE2 operator-	(	PoseSE2	lhs,
		const PoseSE2 &	rhs
	)

friend

Arithmetic operator overload for subtractions.

Parameters

lhs	First term
rhs	Second term

Definition at line 383 of file pose_se2.h.

◆ operator<<

std::ostream& operator<<	(	std::ostream &	stream,
		const PoseSE2 &	pose
	)

friend

Output stream operator.

Parameters

stream	output stream
pose	to be used

Definition at line 421 of file pose_se2.h.

Member Data Documentation

◆ _position

Eigen::Vector2d teb_local_planner::PoseSE2::_position

private

Definition at line 432 of file pose_se2.h.

◆ _theta

double teb_local_planner::PoseSE2::_theta

private

Definition at line 433 of file pose_se2.h.

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

pose_se2.h

Public Member Functions

Private Attributes

Arithmetic operations for which operators are not always reasonable

Operator overloads / Allow some arithmetic operations

Detailed Description

Constructor & Destructor Documentation

◆ PoseSE2() [1/6]

◆ PoseSE2() [2/6]

◆ PoseSE2() [3/6]

◆ PoseSE2() [4/6]

◆ PoseSE2() [5/6]

◆ PoseSE2() [6/6]

◆ ~PoseSE2()

Member Function Documentation

◆ average()

◆ averageInPlace()

◆ operator+=()

◆ operator-=()

◆ operator=()

◆ orientationUnitVec()

◆ plus()

◆ position() [1/2]

◆ position() [2/2]

◆ rotateGlobal()

◆ scale()

◆ setZero()

◆ theta() [1/2]

◆ theta() [2/2]

◆ toPoseMsg()

◆ x() [1/2]

◆ x() [2/2]

◆ y() [1/2]

◆ y() [2/2]

Friends And Related Function Documentation

◆ operator* [1/2]

◆ operator* [2/2]

◆ operator+

◆ operator-

◆ operator<<

Member Data Documentation

◆ _position

◆ _theta