Public Member Functions | Private Attributes
teb_local_planner::PoseSE2 Class Reference

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>

List of all members.

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

PoseSE2operator= (const PoseSE2 &rhs)
 Asignment operator.
PoseSE2operator+= (const PoseSE2 &rhs)
 Compound assignment operator (addition)
PoseSE2operator-= (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

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:
position2D position vector
thetaangle 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:
xx-coordinate
yy-coordinate
thetayaw angle in rad

Definition at line 89 of file pose_se2.h.

Construct pose using a geometry_msgs::Pose.

Parameters:
posegeometry_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:
posetf::Pose object

Definition at line 111 of file pose_se2.h.

teb_local_planner::PoseSE2::PoseSE2 ( const PoseSE2 pose) [inline]

Copy constructor.

Parameters:
posePoseSE2 instance

Definition at line 122 of file pose_se2.h.

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:
pose1first pose to consider
pose2second 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:
pose1first pose to consider
pose2second 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:
rhsaddend

Definition at line 314 of file pose_se2.h.

PoseSE2& teb_local_planner::PoseSE2::operator-= ( const PoseSE2 rhs) [inline]

Compound assignment operator (subtraction)

Parameters:
rhsvalue to subtract

Definition at line 335 of file pose_se2.h.

PoseSE2& teb_local_planner::PoseSE2::operator= ( const PoseSE2 rhs) [inline]

Asignment operator.

Parameters:
rhsPoseSE2 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_array3D 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:
anglethe angle defining the 2d rotation
adjust_thetaif 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:
factorscale factor

Definition at line 227 of file pose_se2.h.

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.

Convert PoseSE2 to a geometry_msgs::Pose.

Parameters:
[out]posePose 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:
posepose to scale
scalarfactor 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:
scalarfactor to multiply with
posepose 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:
lhsFirst addend
rhsSecond addend

Definition at line 326 of file pose_se2.h.

PoseSE2 operator- ( PoseSE2  lhs,
const PoseSE2 rhs 
) [friend]

Arithmetic operator overload for subtractions.

Parameters:
lhsFirst term
rhsSecond term

Definition at line 347 of file pose_se2.h.

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

Output stream operator.

Parameters:
streamoutput stream
poseto 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.

Definition at line 397 of file pose_se2.h.


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


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Sat Jun 8 2019 20:21:34