util Namespace Reference

A collection of utility functions and classes with common use-cases. More...

Classes
class	AsyncTask
	An abstract class that can be overridden to run long-running tasks and encapsulate task-related data. More...
class	latch
	Implementation of a countdown latch for threading synchronization. More...
class	PeriodicScheduler
	Uses a single thread to periodically invoke callbacks at a given frequency. More...
class	PeriodicTask
	Implements a task that executes a function periodically. More...
class	RAIIHelper
	A helper class for executing a function when leaving a scope, in RAII-style. More...
class	ScopedTimer
	A utility class that helps with timing. More...
class	Watchdog
	Implements a thread-safe watchdog. More...

Typedefs
using	json = nlohmann::json
using	val_t = json::value_t
using	dseconds = std::chrono::duration<double, std::chrono::seconds::period>

Functions
std::string	to_string (control::DriveMode mode)
bool	almostEqual (double a, double b, double threshold=1e-6)
	Check if two numbers are approximately equal.
template<>
std::string	to_string< bool > (const bool &val)
	Converts a boolean to a string.
frozen::string	freezeStr (const std::string &str)
	Convert the given std::string to a frozen::string.
template<typename K, typename V>
std::unordered_set< K >	keySet (const std::unordered_map< K, V > &input)
	Get the keys of the given map.
template<typename T>
std::string	to_string (const T &val)
	Convert the given value to a string.
template<>
std::string	to_string< bool > (const bool &val)
	Converts a boolean to a string.
template<typename T, typename U>
std::tuple< T, U >	pairToTuple (const std::pair< T, U > &pair)
	Converts a pair to a tuple.
bool	hasKey (const json &j, const std::string &key)
	Check if the given json object has the given key.
bool	validateKey (const json &j, const std::string &key, const val_t &type)
	Check if the given json object has the given key with the given type.
bool	validateKey (const json &j, const std::string &key, const std::unordered_set< val_t > &types)
	Check if the given json object has the given key, with a type in the given set of types.
bool	validateOneOf (const json &j, const std::string &key, const std::unordered_set< std::string > &vals)
	Check if the value in the given json object at the given key is a string in the given set of allowed values.
bool	validateRange (const json &j, const std::string &key, double min, double max)
	Check if the value in the given json object at the given key is a floating-point number between min and max, inclusive.
Eigen::MatrixXd	numericalJacobian (const std::function< Eigen::VectorXd(const Eigen::VectorXd &)> &func, const Eigen::VectorXd &x, int outputDim)
	Estimate the jacobian of a multivariate function using finite differences.
long	getNormalSeed ()
	Get the random seed used for util::stdn().
double	stdn (int thread_id)
	Sample from the standard normal distribution.
uint64_t	getUnixTime ()
template<typename Rep, typename Period>
double	durationToSec (std::chrono::duration< Rep, Period > dur)
	Convert a duration to seconds, as a double.
double	quatToHeading (double qw, double qx, double qy, double qz)
	Extract the heading from a quaternion.
double	quatToHeading (const Eigen::Quaterniond &quat)
	Extract the heading from a quaternion.
Eigen::Quaterniond	eulerAnglesToQuat (const navtypes::eulerangles_t &rpy)
	Convert euler angles to a quaternion representation.
points_t	transformReadings (const navtypes::points_t &ps, const navtypes::transform_t &tf)
	Convert a set of points to a different reference frame.
trajectory_t	transformTraj (const navtypes::trajectory_t &traj, const navtypes::transform_t &tf)
	Convert a trajectory to a different reference frame.
bool	collides (const navtypes::transform_t &tf, const navtypes::points_t &lms, double radius)
	Check for intersection between a pose and a point cloud.
transform_t	toTransformRotateFirst (double x, double y, double theta)
	Creates a rigid transform that takes a point from the map-frame to a frame that was rotated CCW by theta and shifted (x,y) along the new axes.
double	closestHeading (double theta, double prev_theta)
	Get an equivalent heading that is within π radians of another heading.
pose_t	toPose (const navtypes::transform_t &trf, double prev_theta)
	Given a transform, gives a robot pose that would have that transform.
transform_t	toTransform (const navtypes::pose_t &pose)
	Convert a pose to a rigid transform.
std::string	to_string (robot::types::jointid_t joint)
std::string	to_string (robot::types::mountedperipheral_t peripheral)
std::string	to_string (robot::types::servoid_t servo)
std::string	to_string (const robot::types::CameraID &id)

Variables
constexpr double	epsilon = 1e-5
	A small positive value, near zero.

Detailed Description

A collection of utility functions and classes with common use-cases.

Function Documentation

almostEqual()

bool util::almostEqual	(	double	a,
		double	b,
		double	threshold = 1e-6 )

Check if two numbers are approximately equal.

Parameters

a	The first number.
b	The second number.
threshold	If |a-b|≥threshold then they are not approximately equal.

Returns

true iff |a-b|<threshold.

closestHeading()

double util::closestHeading	(	double	theta,
		double	prev_theta )

Get an equivalent heading that is within π radians of another heading.

Parameters

theta	The heading to get the equivalent heading for.
prev_theta	The heading that the returned heading is in the neighborhood of.

Returns

A heading equivalent to theta that is within π radians of prev_theta.

collides()

bool util::collides	(	const navtypes::transform_t &	tf,
		const navtypes::points_t &	lms,
		double	radius )

Check for intersection between a pose and a point cloud.

Checks if any point of lms is within radius distance of the robot.

Parameters

tf	The robot transform.
lms	The points to check against for intersection.
radius	The minimum distance from the robot to a point to not count as an intersection.

Returns

If any point of lms is within radius distance of the robot.

durationToSec()

template<typename Rep, typename Period>

double util::durationToSec

(

std::chrono::duration< Rep, Period >

dur

)

Convert a duration to seconds, as a double.

Template Parameters

Rep	The rep of the duration.
Period	The period of the duration.

Parameters

dur	The duration to convert.

Returns

double The length of the duration, in seconds.

eulerAnglesToQuat()

Eigen::Quaterniond util::eulerAnglesToQuat

(

const navtypes::eulerangles_t &

rpy

)

Convert euler angles to a quaternion representation.

Parameters

rpy	The euler angles to convert.

Returns

Eigen::Quaterniond The same orientation represented as a quaternion.

getNormalSeed()

long util::getNormalSeed

(

)

Get the random seed used for util::stdn().

Returns

The random seed.

keySet()

template<typename K, typename V>

std::unordered_set< K > util::keySet

(

const std::unordered_map< K, V > &

input

)

Get the keys of the given map.

Parameters

An	unordered map.

Returns

The keys of the given map, as an unordered set.

numericalJacobian()

Eigen::MatrixXd util::numericalJacobian	(	const std::function< Eigen::VectorXd(const Eigen::VectorXd &)> &	func,
		const Eigen::VectorXd &	x,
		int	outputDim )

Estimate the jacobian of a multivariate function using finite differences.

Parameters

func	The function to estimate the jacobian of.
x	The point around which to calculate the jacobian.
outputDim	The dimension of the output space of func.

Returns

Eigen::MatrixXd An n-by-m matrix where n is the output dimension and m is the input dimension of func.

pairToTuple()

template<typename T, typename U>

std::tuple< T, U > util::pairToTuple

(

const std::pair< T, U > &

pair

)

Converts a pair to a tuple.

Elements are copied to the returned tuple.

Template Parameters

T	The type of the first element.
U	The type of the second element.

Parameters

pair	The pair to convert to a tuple.

Returns

std::tuple<T, U> The converted tuple.

quatToHeading() [1/2]

double util::quatToHeading

(

const Eigen::Quaterniond &

quat

)

Extract the heading from a quaternion.

Given a quaternion defined by out coordinate system, extract the CCW heading, in radians.

Parameters

quat	The quaternion to extract the heading from. This does not need to be normalized.

Returns

double The CCW heading, in radians.

quatToHeading() [2/2]

double util::quatToHeading	(	double	qw,
		double	qx,
		double	qy,
		double	qz )

Extract the heading from a quaternion.

Given a quaternion defined by out coordinate system, extract the CCW heading, in radians.

Parameters

qw	The w component of the quaternion.
qx	The x component of the quaternion.
qy	The y component of the quaternion.
qz	The z component of the quaternion.

Returns

double The CCW heading, in radians.

stdn()

double util::stdn

(

int

thread_id

)

Sample from the standard normal distribution.

The thread_id is used to choose which random number generator to use. This is important when trying to rerun a particular random seed; each thread needs its own dedicated sequence of random numbers.

Parameters

thread_id

Either 0 or 1, denoting which random number generator use.

Returns

double

to_string()

template<typename T>

std::string util::to_string

(

const T &

val

)

Convert the given value to a string.

This method is necessary because we cannot extend the std namespace; having our own method allows us to extend it with template specializations whenever we want, and have it "fall back" to using the std version when no specialization is available.

Parameters

val	The value to convert to string.

Template Parameters

The	type of the value to convert to string.

Returns

A string representation of that value, as a std::string. The exact representation of the value is up to the implementation.

to_string< bool >() [1/2]

template<>

std::string util::to_string< bool >

(

const bool &

val

)

Converts a boolean to a string.

Parameters

val	The value to get the string representation of.

Returns

"true" iff val, otherwise "false".

to_string< bool >() [2/2]

template<>

std::string util::to_string< bool >

(

const bool &

val

)

Converts a boolean to a string.

Parameters

val	The value to get the string representation of.

Returns

"true" iff val, otherwise "false".

toPose()

navtypes::pose_t util::toPose	(	const navtypes::transform_t &	trf,
		double	prev_theta )

Given a transform, gives a robot pose that would have that transform.

That is, the x,y location and angle theta of the robot such that left-multiplying a map-frame coordinate by trf will give you the robot-frame coordinate.

Parameters

trf	The rigid transform representing a robot position. This transform would take points from the map frame to the robot frame.
prev_theta	Since heading is modular, the returned heading value will be chosen to be within 2π radians of this parameter.

Returns

The robot pose.

Warning

THIS IS AN INVERSE OF util::toTransform, NOT util::toTransformRotateFirst. The parameters of the latter are not a robot pose (rotation and translation operations do not commute).

toTransform()

navtypes::transform_t util::toTransform

(

const navtypes::pose_t &

pose

)

Convert a pose to a rigid transform.

Calculate the rigid transform associated with the given pose. This rigid transform takes points from the global frame to the robot frame.

Parameters

pose	The current robot pose.

Returns

navtypes::transform_t The rigid transform denoting the pose of the robot.

toTransformRotateFirst()

navtypes::transform_t util::toTransformRotateFirst	(	double	x,
		double	y,
		double	theta )

Creates a rigid transform that takes a point from the map-frame to a frame that was rotated CCW by theta and shifted (x,y) along the new axes.

This can also be viewed as rotating a map-frame point CW by theta and then shifting by (-x,-y) along the map axes.

Parameters

x	The x-coordinate in map-space of the origin of the new frame.
y	The y-coordinate in map-space of the origin of the new frame.
theta	The CCW angle between the new frame and the old frame.

Returns

A rigid transform that changes frames as described.

transformReadings()

navtypes::points_t util::transformReadings	(	const navtypes::points_t &	ps,
		const navtypes::transform_t &	tf )

Convert a set of points to a different reference frame.

Applies tf^-1 to every element of ps and returns the transformed points.

Parameters

ps	The points to convert between reference frames.
tf	The transform whose inverse will be applied to the points.

Returns

Points such that the the i-th point is equivalent to tf^-1*ps[i].

Note

The inverse of the given transform is applied to every point, not the transform itself.

transformTraj()

navtypes::trajectory_t util::transformTraj	(	const navtypes::trajectory_t &	traj,
		const navtypes::transform_t &	tf )

Convert a trajectory to a different reference frame.

Applies tf on the right for every element of traj and returns the transformed trajectory.

Parameters

traj	The trajectory to transform.
tf	The transform to apply on the right to every element of traj.

Returns

The transformed trajectory, such that the i-th element is traj[i]*tf.