39 #include <Eigen/Dense> 58 template<
typename T,
typename ...Args>
61 return std::unique_ptr<T>(
new T( std::forward<Args>(args)... ) );
66 typedef std::chrono::high_resolution_clock high_resolution_clock;
67 typedef std::chrono::nanoseconds nanoseconds;
80 _start = high_resolution_clock::now();
84 return static_cast<double>(std::chrono::duration_cast<nanoseconds>(high_resolution_clock::now() - _start).count());
94 template <
typename T,
typename Traits>
95 friend std::basic_ostream<T, Traits>& operator<<(std::basic_ostream<T, Traits>& out,
const PosixTimer& timer)
100 high_resolution_clock::time_point _start;
108 template <
typename Type>
115 std::string
str()
const {
return stream_.str(); }
116 operator std::string ()
const {
return stream_.str(); }
125 std::stringstream stream_;
140 std::runtime_error(arg) {
141 std::ostringstream o;
142 o <<
"\033[31m" << file <<
":" << line <<
": " << arg <<
"\033[0m";
146 const char *
what()
const throw() {
227 template <
typename Derived>
228 void assertSize(
const Eigen::EigenBase<Derived>& a,
const Eigen::EigenBase<Derived>& b)
230 if(a.cols() == b.cols() && a.rows() == b.rows())
232 throw std::length_error(
Formatter() <<
"Size mismatched, provided size (" << a.rows() <<
" , " << a.cols() <<
"), but have size (" << b.rows() <<
" , " << b.cols() <<
")");
239 throw std::length_error(
Formatter() <<
"Vector size is " << a.size() <<
", but should be (" << s <<
")");
242 template<
class T>
bool exists(
const T& t,std::list< T > l){
243 return std::find(l.begin(),l.end(),t) != l.end();
246 template <
typename Key,
typename Vals>
247 bool key_exists(
const std::map<Key,Vals>& container,
const Key& key)
249 auto it = container.begin();
250 while(it != container.end())
double elapsed() const
Definition: Utils.h:90
bool exists(const T &t, std::list< T > l)
Definition: Utils.h:242
void assertSize(const Eigen::EigenBase< Derived > &a, const Eigen::EigenBase< Derived > &b)
Definition: Utils.h:228
double elapsedMs() const
Definition: Utils.h:86
void orca_throw(const std::string &arg)
Definition: Utils.h:151
std::unique_ptr< T > make_unique(Args &&...args)
Definition: Utils.h:59
~orca_exception()
Definition: Utils.h:145
const char * what() const
Definition: Utils.h:146
friend std::basic_ostream< T, Traits > & operator<<(std::basic_ostream< T, Traits > &out, const PosixTimer &timer)
Definition: Utils.h:95
PosixTimer(bool start_now=false)
Definition: Utils.h:73
bool key_exists(const std::map< Key, Vals > &container, const Key &key)
Definition: Utils.h:247
double elapsedNs() const
Definition: Utils.h:82
const double nsToS
Definition: Utils.h:71
Definition: CartesianAccelerationPID.h:44
const double nsToMs
Definition: Utils.h:70
void start()
Definition: Utils.h:78
orca_exception(const std::string &arg, const char *file, int line)
Definition: Utils.h:139