utilities.h File Reference

#include <algorithm>
#include <map>
#include <random>
#include <unordered_map>
#include <vector>
#include "cppTypes.h"

Include dependency graph for utilities.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Functions
template<typename T >
bool	fpEqual (T a, T b, T tol)
template<typename T >
bool	vectorEqual (const std::vector< T > &a, const std::vector< T > &b)
template<typename T >
T	coerce (T in, T min, T max)
template<typename T >
T	deadband (T x, T range)
template<typename T >
void	eigenDeadband (Eigen::MatrixBase< T > &v, typename T::Scalar band)
template<typename T >
int	sgn (T val)
template<typename T >
void	fillEigenWithRandom (Eigen::MatrixBase< T > &v, std::mt19937 &gen, std::uniform_real_distribution< typename T::Scalar > &dist)
template<typename T >
T	generator_gaussian_noise (T mean, T var)
template<typename T1 , typename T2 >
bool	uMapContains (const std::unordered_map< T1, T2 > &set, T1 key)
template<typename T1 , typename T2 >
bool	mapContains (const std::map< T1, T2 > &set, T1 key)
template<typename T >
std::string	numberToString (T number)
template<typename T >
T	mapToRange (T x, T inputMin, T inputMax, T outputMin, T outputMax)
template<typename T >
std::string	eigenToString (Eigen::MatrixBase< T > &value)
static std::string	boolToString (bool b)
void	writeStringToFile (const std::string &fileName, const std::string &fileData)
std::string	getCurrentTimeAndDate ()
std::string	getConfigDirectoryPath ()
template<typename T >
void	EulerZYX_2_SO3 (const Vec3< T > &euler_zyx, Mat3< T > &SO3)
template<typename T >
T	smooth_change (T ini, T end, T moving_duration, T curr_time)
template<typename T >
T	smooth_change_vel (T ini, T end, T moving_duration, T curr_time)
template<typename T >
T	smooth_change_acc (T ini, T end, T moving_duration, T curr_time)
template<typename T >
T	stringToNumber (const std::string &str)
template<typename T >
T	stringToNumber (const char *str)
template<typename T >
Vec3< T >	stringToVec3 (const std::string &str)
std::string	getLcmUrl (s64 ttl)

Function Documentation

static std::string boolToString ( bool  b )

inlinestatic

Definition at line 154 of file utilities.h.

References getConfigDirectoryPath(), getCurrentTimeAndDate(), and writeStringToFile().

                                             {
  return std::string(b ? "true" : "false");
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

T coerce	(	T	in,
		T	min,
		T	max
	)

Coerce in to be between min and max

Definition at line 35 of file utilities.h.

                             {
  if (in < min) {
    in = min;
  }
  if (in > max) {
    in = max;
  }
  return in;
}

Here is the caller graph for this function:

template<typename T >

T deadband	(	T	x,
		T	range
	)

Definition at line 46 of file utilities.h.

                         {
  if (x < range && x > -range) x = T(0);
  return x;
}

Here is the caller graph for this function:

template<typename T >

void eigenDeadband	(	Eigen::MatrixBase< T > &	v,
		typename T::Scalar	band
	)

Definition at line 52 of file utilities.h.

References deadband().

                                                                 {
  for (size_t i = 0; i < T::RowsAtCompileTime; i++) {
    for (size_t j = 0; j < T::ColsAtCompileTime; j++) {
      v(i, j) = deadband(v(i, j), band);
    }
  }
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

std::string eigenToString

(

Eigen::MatrixBase< T > &

value

)

Definition at line 148 of file utilities.h.

                                                   {
  std::stringstream ss;
  ss << value;
  return ss.str();
}

Here is the caller graph for this function:

template<typename T >

void EulerZYX_2_SO3	(	const Vec3< T > &	euler_zyx,
		Mat3< T > &	SO3
	)

Get the rotation matrix coincide with euler angle Intrisic ZYX rotation

Definition at line 168 of file utilities.h.

                                                            {
  Mat3<T> Mat3_Z, Mat3_Y, Mat3_X;
  Mat3_Z << cos(euler_zyx[0]), -sin(euler_zyx[0]), 0, sin(euler_zyx[0]),
      cos(euler_zyx[0]), 0, 0, 0, 1;
  Mat3_Y << cos(euler_zyx[1]), 0, sin(euler_zyx[1]), 0, 1, 0,
      -sin(euler_zyx[1]), 0, cos(euler_zyx[1]);
  Mat3_X << 1, 0, 0, 0, cos(euler_zyx[2]), -sin(euler_zyx[2]), 0,
      sin(euler_zyx[2]), cos(euler_zyx[2]);

  SO3 = Mat3_Z * Mat3_Y * Mat3_X;
}

template<typename T >

void fillEigenWithRandom	(	Eigen::MatrixBase< T > &	v,
		std::mt19937 &	gen,
		std::uniform_real_distribution< typename T::Scalar > &	dist
	)

Fill an eigen type with random numbers from a random generator and uniform real distribution. TODO: is there a way to make this work nicely with normal distributions too?

Definition at line 75 of file utilities.h.

                                                          {
  for (size_t i = 0; i < T::RowsAtCompileTime; i++) {
    for (size_t j = 0; j < T::ColsAtCompileTime; j++) {
      v(i, j) = dist(gen);
    }
  }
}

Here is the caller graph for this function:

template<typename T >

bool fpEqual	(	T	a,
		T	b,
		T	tol
	)

Are two floating point values almost equal?

Definition at line 15 of file utilities.h.

                              {
  return std::abs(a - b) <= tol;
}

Here is the caller graph for this function:

template<typename T >

T generator_gaussian_noise	(	T	mean,
		T	var
	)

Generate a random number following normal distribution

Definition at line 89 of file utilities.h.

                                          {
  static bool hasSpare = false;
  static T rand1, rand2;

  if (hasSpare) {
    hasSpare = false;
    return mean + sqrt(var * rand1) * sin(rand2);
  }
  hasSpare = true;

  rand1 = rand() / ((T)RAND_MAX);
  if (rand1 < 1e-100) rand1 = 1e-100;
  rand1 = -2 * log(rand1);
  rand2 = rand() / ((T)RAND_MAX) * M_PI * 2.;

  // printf("rand: %f, %f\n", rand1, rand2);
  return mean + sqrt(var * rand1) * cos(rand2);
}

std::string getConfigDirectoryPath

(

)

Todo: do something better to keep track of where we are relative to the config directory

Definition at line 30 of file utilities.cpp.

{ return "../config/"; }

Here is the caller graph for this function:

std::string getCurrentTimeAndDate

(

)

Definition at line 18 of file utilities.cpp.

                                  {
  auto t = std::time(nullptr);
  auto tm = *std::localtime(&t);
  std::ostringstream ss;
  ss << std::put_time(&tm, "%c");
  return ss.str();
}

Here is the caller graph for this function:

std::string getLcmUrl

(

s64

ttl

)

Definition at line 32 of file utilities.cpp.

                             {
  assert(ttl >= 0 && ttl <= 255);
  return "udpm://239.255.76.67:7667?ttl=" + std::to_string(ttl);
}

template<typename T1 , typename T2 >

bool mapContains	(	const std::map< T1, T2 > &	set,
		T1	key
	)

Does the unordered map contain the given element?

Definition at line 120 of file utilities.h.

                                                    {
  return set.find(key) != set.end();
}

Here is the caller graph for this function:

template<typename T >

T mapToRange	(	T	x,
		T	inputMin,
		T	inputMax,
		T	outputMin,
		T	outputMax
	)

map value x in (inputMin, inputMax) to (outputMin, outputMax)

Definition at line 142 of file utilities.h.

                                                                    {
  return outputMin +
         (x - inputMin) * (outputMax - outputMin) / (inputMax - inputMin);
}

Here is the caller graph for this function:

template<typename T >

std::string numberToString

(

T

number

)

Convert a floating point number to a string. Is preferable over std::to_string because this uses scientific notation and won't truncate small/large numbers.

Definition at line 130 of file utilities.h.

                                   {
  static_assert(std::is_floating_point<T>::value,
                "numberToString must use a floating point type!");
  char buffer[100];
  sprintf(buffer, "%g", number);
  return std::string(buffer);
}

Here is the caller graph for this function:

template<typename T >

int sgn

(

T

val

)

Get the sign of a number 1 for positive, 0 for 0, -1 for negative...

Definition at line 65 of file utilities.h.

               {
  return (T(0) < val) - (val < T(0));
}

Here is the caller graph for this function:

template<typename T >

T smooth_change	(	T	ini,
		T	end,
		T	moving_duration,
		T	curr_time
	)

Definition at line 182 of file utilities.h.

                                                              {
  if (curr_time > moving_duration) {
    return end;
  }
  return (ini +
          (end - ini) * 0.5 * (1 - cos(curr_time / moving_duration * M_PI)));
}

template<typename T >

T smooth_change_acc	(	T	ini,
		T	end,
		T	moving_duration,
		T	curr_time
	)

Definition at line 200 of file utilities.h.

                                                                  {
  if (curr_time > moving_duration) {
    return 0.0;
  }
  return ((end - ini) * 0.5 * (M_PI / moving_duration) *
          (M_PI / moving_duration) * cos(curr_time / moving_duration * M_PI));
}

template<typename T >

T smooth_change_vel	(	T	ini,
		T	end,
		T	moving_duration,
		T	curr_time
	)

Definition at line 191 of file utilities.h.

                                                                  {
  if (curr_time > moving_duration) {
    return 0.0;
  }
  return ((end - ini) * 0.5 * (M_PI / moving_duration) *
          sin(curr_time / moving_duration * M_PI));
}

template<typename T >

T stringToNumber

(

const std::string &

str

)

Definition at line 209 of file utilities.h.

                                       {
  static_assert(std::is_same<T, double>::value || std::is_same<T, float>::value,
                "stringToNumber only works for double/float");

  if (std::is_same<T, double>::value) {
    return std::stod(str);
  } else if (std::is_same<T, float>::value) {
    return std::stof(str);
  }
}

template<typename T >

T stringToNumber

(

const char *

str

)

Definition at line 221 of file utilities.h.

                                  {
  return stringToNumber<T>(std::string(str));
}

template<typename T >

Vec3<T> stringToVec3

(

const std::string &

str

)

Definition at line 226 of file utilities.h.

References getLcmUrl().

                                           {
  Vec3<T> v;
  size_t i = 0;

  // seek past whitespace
  while (str.at(i) == ' ') i++;

  if (str.at(i) == '[') {
    i++;
  } else {
    throw std::runtime_error("stringToVec3 didn't find open bracket");
  }

  // seek past whitespace
  while (str.at(i) == ' ') i++;
  size_t start = i;

  // seek to end of first number
  while (str.at(i) != ',') i++;
  v[0] = stringToNumber<T>(str.substr(start, i - start));
  i++;

  while (str.at(i) == ' ') i++;
  start = i;
  while (str.at(i) != ',') i++;
  v[1] = stringToNumber<T>(str.substr(start, i - start));
  i++;

  while (str.at(i) == ' ') i++;
  start = i;
  while (str.at(i) != ']') i++;
  v[2] = stringToNumber<T>(str.substr(start, i - start));
  return v;
}

Here is the call graph for this function:

template<typename T1 , typename T2 >

bool uMapContains	(	const std::unordered_map< T1, T2 > &	set,
		T1	key
	)

Does the unordered map contain the given element?

Definition at line 112 of file utilities.h.

                                                               {
  return set.find(key) != set.end();
}

Here is the caller graph for this function:

template<typename T >

bool vectorEqual	(	const std::vector< T > &	a,
		const std::vector< T > &	b
	)

Are two std::vectors equal?

Definition at line 23 of file utilities.h.

                                                               {
  if (a.size() != b.size()) return false;
  for (size_t i = 0; i < a.size(); i++) {
    if (a[i] != b[i]) return false;
  }
  return true;
}

Here is the caller graph for this function:

void writeStringToFile	(	const std::string &	fileName,
		const std::string &	fileData
	)

Definition at line 7 of file utilities.cpp.

                                                  {
  FILE* fp = fopen(fileName.c_str(), "w");
  if (!fp) {
    printf("Failed to fopen %s\n", fileName.c_str());
    throw std::runtime_error("Failed to open file");
  }
  fprintf(fp, "%s", fileData.c_str());
  fclose(fp);
}

Here is the caller graph for this function: