utils

Some helper functions to be used in different components of the software architecture.

namespace slope_constrained_planner

Typedefs

using Scalar = float
using Pose3 = Eigen::Transform<Scalar, 3, Eigen::Affine>

Functions

inline Pose3 Pose3FromSE3(const ob::State *state)

Gets position of state from its SE3 representation.

Parameters:

state – State info

Returns:

Position of state

inline Pose3 Pose3FromXYZ(Scalar x, Scalar y, Scalar z)

Gets the position of state from its x-y-z representation.

Parameters:

  • x – State’s x value

  • y – State’s y value

  • z – State’s z value

Returns:

Position of the state

inline double lateralDistance(const ob::State *from, const ob::State *to)

Gets the lateral distance between two states.

Parameters:

  • from – Starting state

  • to – Ending state

Returns:

Lateral distance between from and to states

template<typename T>
inline T getRollFromQuat(T w, T x, T y, T z)

Gets roll angle from quaternion represntation.

Parameters:

  • w – Quaternion w value

  • x – Quaternion x value

  • y – Quaternion y value

  • z – Quaternion z value

Returns:

Roll angle

template<typename T>
inline T getPitchFromQuat(T w, T x, T y, T z)

Gets pitch angle from quaternion represntation.

Parameters:

  • w – Quaternion w value

  • x – Quaternion x value

  • y – Quaternion y value

  • z – Quaternion z value

Returns:

Pitch angle

template<typename T>
inline T getYawFromQuat(T w, T x, T y, T z)

Gets yaw angle from quaternion represntation.

Parameters:

  • w – Quaternion w value

  • x – Quaternion x value

  • y – Quaternion y value

  • z – Quaternion z value

Returns:

Yaw angle

inline Scalar getYawFromSO3(const ob::SO3StateSpace::StateType &s)

Gets yaw angle from SO3 represntation.

Parameters:

s – State data

Returns:

Yaw angle

inline Scalar getRollFromSO3(const ob::SO3StateSpace::StateType &s)

Gets roll angle from SO3 represntation.

Parameters:

s – State data

Returns:

Roll angle

inline Scalar getPitchFromSO3(const ob::SO3StateSpace::StateType &s)

Gets pitch angle from SO3 represntation.

Parameters:

s – State data

Returns:

Pitch angle

inline void setSO3FromYaw(ob::SO3StateSpace::StateType &s, double yaw)

Gets SO3 represntation from yaw angle.

Parameters:

  • s – State data

  • yaw – Yaw angle

inline void setSO3FromRPY(ob::SO3StateSpace::StateType &s, double *rpy)

Sets SO3 represntation using RPY representation.

Parameters:

  • s – State data

  • rpy – Roll, pitch and yaw angles

void estimateNormals(grid_map::GridMap &map, double estimation_radius, const std::string &input_layer, const std::string &output_layer_prefix = "normal")

Estimates the normal vectors in a grid map cell.

Parameters:

  • map – Grid map data

  • estimation_radius – The dimension of estimation

  • input_layer – Name of the input layer

  • output_layer_prefix – Prefix of output layer