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 57 of file pose_se2.h.

Constructor & Destructor Documentation

teb_local_planner::PoseSE2::PoseSE2 ( )

inline

Default constructor.

Definition at line 67 of file pose_se2.h.

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 77 of file pose_se2.h.

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 89 of file pose_se2.h.

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 100 of file pose_se2.h.

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

inline

Construct pose using a tf::Pose.

Parameters

pose	tf::Pose object

Definition at line 111 of file pose_se2.h.

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

inline

Copy constructor.

Parameters

pose	PoseSE2 instance

Definition at line 122 of file pose_se2.h.

teb_local_planner::PoseSE2::~PoseSE2 ( )

inline

Destructs the PoseSE2.

Definition at line 134 of file pose_se2.h.

Member Function Documentation

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 266 of file pose_se2.h.

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 252 of file pose_se2.h.

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

inline

Compound assignment operator (addition)

Parameters

rhs addend

Definition at line 314 of file pose_se2.h.

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

inline

Compound assignment operator (subtraction)

Parameters

rhs	value to subtract

Definition at line 335 of file pose_se2.h.

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 300 of file pose_se2.h.

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 215 of file pose_se2.h.

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 238 of file pose_se2.h.

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 145 of file pose_se2.h.

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 152 of file pose_se2.h.

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 279 of file pose_se2.h.

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 227 of file pose_se2.h.

void teb_local_planner::PoseSE2::setZero ( )

inline

Set pose to [0,0,0].

Definition at line 193 of file pose_se2.h.

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 182 of file pose_se2.h.

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 188 of file pose_se2.h.

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 203 of file pose_se2.h.

double& teb_local_planner::PoseSE2::x ( )

inline

Access the x-coordinate the pose.

Returns: reference to the x-coordinate

Definition at line 158 of file pose_se2.h.

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 164 of file pose_se2.h.

double& teb_local_planner::PoseSE2::y ( )

inline

Access the y-coordinate the pose.

Returns: reference to the y-coordinate

Definition at line 170 of file pose_se2.h.

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 176 of file pose_se2.h.

Friends And Related Function Documentation

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 359 of file pose_se2.h.

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 373 of file pose_se2.h.

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

friend

Arithmetic operator overload for additions.

Parameters

lhs	First addend
rhs	Second addend

Definition at line 326 of file pose_se2.h.

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

friend

Arithmetic operator overload for subtractions.

Parameters

lhs	First term
rhs	Second term

Definition at line 347 of file pose_se2.h.

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

friend

Output stream operator.

Parameters

stream	output stream
pose	to be used

Definition at line 385 of file pose_se2.h.

Member Data Documentation

Eigen::Vector2d teb_local_planner::PoseSE2::_position

private

Definition at line 396 of file pose_se2.h.

double teb_local_planner::PoseSE2::_theta

private

Definition at line 397 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

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation