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.
Construct PoseSE2 instances
	PoseSE2 ()
	Default constructor.
	PoseSE2 (const Eigen::Ref< const Eigen::Vector2d > &position, double theta)
	Construct pose given a position vector and an angle theta.
	PoseSE2 (double x, double y, double theta)
	Construct pose using single components x, y, and the yaw angle.
	PoseSE2 (const geometry_msgs::Pose &pose)
	Construct pose using a geometry_msgs::Pose.
	PoseSE2 (const tf::Pose &pose)
	Construct pose using a tf::Pose.
	PoseSE2 (const PoseSE2 &pose)
	Copy constructor.
Access and modify values
Eigen::Vector2d &	position ()
	Access the 2D position part.
const Eigen::Vector2d &	position () const
	Access the 2D position part (read-only)
double &	x ()
	Access the x-coordinate the pose.
const double &	x () const
	Access the x-coordinate the pose (read-only)
double &	y ()
	Access the y-coordinate the pose.
const double &	y () const
	Access the y-coordinate the pose (read-only)
double &	theta ()
	Access the orientation part (yaw angle) of the pose.
const double &	theta () const
	Access the orientation part (yaw angle) of the pose (read-only)
void	setZero ()
	Set pose to [0,0,0].
void	toPoseMsg (geometry_msgs::Pose &pose) const
	Convert PoseSE2 to a geometry_msgs::Pose.
Eigen::Vector2d	orientationUnitVec () const
	Return the unit vector of the current orientation.
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].
void	plus (const double *pose_as_array)
	Increment the pose by adding a double[3] array The angle is normalized afterwards.
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.
void	rotateGlobal (double angle, bool adjust_theta=true)
	Rotate pose globally.
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.
Operator overloads / Allow some arithmetic operations
PoseSE2 &	operator= (const PoseSE2 &rhs)
	Asignment operator.
PoseSE2 &	operator+= (const PoseSE2 &rhs)
	Compound assignment operator (addition)
PoseSE2 &	operator-= (const PoseSE2 &rhs)
	Compound assignment operator (subtraction)
PoseSE2	operator+ (PoseSE2 lhs, const PoseSE2 &rhs)
	Arithmetic operator overload for additions.
PoseSE2	operator- (PoseSE2 lhs, const PoseSE2 &rhs)
	Arithmetic operator overload for subtractions.
PoseSE2	operator* (PoseSE2 pose, double scalar)
	Multiply pose with scalar and return copy without normalizing theta This operator is useful for calculating velocities ...
PoseSE2	operator* (double scalar, PoseSE2 pose)
	Multiply pose with scalar and return copy without normalizing theta This operator is useful for calculating velocities ...
std::ostream &	operator<< (std::ostream &stream, const PoseSE2 &pose)
	Output stream operator.

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
	)		`[inline, static]`

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