depthaiUtility.hpp
Go to the documentation of this file.
1 #pragma once
2 
3 #include <chrono>
4 #ifdef IS_ROS2
5  #include "rclcpp/rclcpp.hpp"
6 #else
7  #include <ros/ros.h>
8 #endif
9 
10 namespace dai {
11 
12 namespace ros {
13 
14 enum LogLevel { DEBUG, INFO, WARN, ERROR, FATAL };
15 
16 #ifdef IS_ROS2
17 
18  #define DEPTHAI_ROS_LOG_STREAM(loggerName, level, isOnce, args) \
19  switch(level) { \
20  case LogLevel::DEBUG: \
21  if(isOnce) { \
22  RCLCPP_DEBUG_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \
23  } else { \
24  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(loggerName), args); \
25  } \
26  break; \
27  case LogLevel::INFO: \
28  if(isOnce) { \
29  RCLCPP_INFO_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \
30  } else { \
31  RCLCPP_INFO_STREAM(rclcpp::get_logger(loggerName), args); \
32  } \
33  break; \
34  case LogLevel::WARN: \
35  if(isOnce) { \
36  RCLCPP_WARN_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \
37  } else { \
38  RCLCPP_WARN_STREAM(rclcpp::get_logger(loggerName), args); \
39  } \
40  break; \
41  case LogLevel::ERROR: \
42  if(isOnce) { \
43  RCLCPP_ERROR_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \
44  } else { \
45  RCLCPP_ERROR_STREAM(rclcpp::get_logger(loggerName), args); \
46  } \
47  break; \
48  case LogLevel::FATAL: \
49  if(isOnce) { \
50  RCLCPP_FATAL_STREAM_ONCE(rclcpp::get_logger(loggerName), args); \
51  } else { \
52  RCLCPP_FATAL_STREAM(rclcpp::get_logger(loggerName), args); \
53  } \
54  break; \
55  }
56 
57 #else
58  #define DEPTHAI_ROS_LOG_STREAM(loggerName, level, isOnce, args) \
59  if(isOnce) { \
60  ROS_LOG_STREAM_ONCE(static_cast<::ros::console::Level>(level), std::string(ROSCONSOLE_NAME_PREFIX) + "." + loggerName, args); \
61  } else { \
62  ROS_LOG_STREAM(static_cast<::ros::console::Level>(level), std::string(ROSCONSOLE_NAME_PREFIX) + "." + loggerName, args); \
63  }
64 #endif
65 
66 // DEBUG stream macros on top of ROS logger
67 #define DEPTHAI_ROS_DEBUG_STREAM(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, LogLevel::DEBUG, false, args)
68 
69 #define DEPTHAI_ROS_DEBUG_STREAM_ONCE(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, LogLevel::DEBUG, true, args)
70 
71 // INFO stream macros on top of ROS logger
72 #define DEPTHAI_ROS_INFO_STREAM(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, LogLevel::INFO, false, args)
73 
74 #define DEPTHAI_ROS_INFO_STREAM_ONCE(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, LogLevel::INFO, true, args)
75 
76 // WARN stream macros on top of ROS logger
77 #define DEPTHAI_ROS_WARN_STREAM(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, LogLevel::WARN, false, args)
78 
79 #define DEPTHAI_ROS_WARN_STREAM_ONCE(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, LogLevel::WARN, true, args)
80 
81 // ERROR stream macros on top of ROS logger
82 #define DEPTHAI_ROS_ERROR_STREAM(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, LogLevel::ERROR, false, args)
83 
84 #define DEPTHAI_ROS_ERROR_STREAM_ONCE(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, LogLevel::ERROR, true, args)
85 
86 // FATAL stream macros on top of ROS logger
87 #define DEPTHAI_ROS_FATAL_STREAM(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, LogLevel::FATAL, false, args)
88 
89 #define DEPTHAI_ROS_FATAL_STREAM_ONCE(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, LogLevel::FATAL, true, args)
90 
91 #ifdef IS_ROS2
92 rclcpp::Time getFrameTime(rclcpp::Time rclBaseTime,
93  std::chrono::time_point<std::chrono::steady_clock> steadyBaseTime,
94  std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration> currTimePoint) {
95  auto elapsedTime = currTimePoint - steadyBaseTime;
96  // uint64_t nSec = rosBaseTime.toNSec() + std::chrono::duration_cast<std::chrono::nanoseconds>(elapsedTime).count();
97  auto rclStamp = rclBaseTime + elapsedTime;
98  // DEPTHAI_ROS_DEBUG_STREAM("PRINT TIMESTAMP: ", "rosStamp -> " << rclStamp << " rosBaseTime -> " << rclBaseTime);
99  return rclStamp;
100 }
101 
102 #else
103 
104 ::ros::Time getFrameTime(::ros::Time rosBaseTime,
105  std::chrono::time_point<std::chrono::steady_clock> steadyBaseTime,
106  std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration> currTimePoint) {
107  auto elapsedTime = currTimePoint - steadyBaseTime;
108  uint64_t nSec = rosBaseTime.toNSec() + std::chrono::duration_cast<std::chrono::nanoseconds>(elapsedTime).count();
109  auto currTime = rosBaseTime;
110  auto rosStamp = currTime.fromNSec(nSec);
111  DEPTHAI_ROS_DEBUG_STREAM("PRINT TIMESTAMP: ", "rosStamp -> " << rosStamp << " rosBaseTime -> " << rosBaseTime);
112  return rosStamp;
113 }
114 #endif
115 
116 template <typename T>
117 T lerp(const T& a, const T& b, const double t) {
118  return a * (1.0 - t) + b * t;
119 }
120 
121 template <typename T>
122 T lerpImu(const T& a, const T& b, const double t) {
123  T res;
124  res.x = lerp(a.x, b.x, t);
125  res.y = lerp(a.y, b.y, t);
126  res.z = lerp(a.z, b.z, t);
127  return res;
128 }
129 
130 } // namespace ros
131 } // namespace dai
dai::ros::lerpImu
T lerpImu(const T &a, const T &b, const double t)
Definition: depthaiUtility.hpp:122
TimeBase< Time, Duration >::fromNSec
Time & fromNSec(uint64_t t)
DEPTHAI_ROS_DEBUG_STREAM
#define DEPTHAI_ROS_DEBUG_STREAM(loggerName, args)
Definition: depthaiUtility.hpp:67
TimeBase< Time, Duration >::toNSec
uint64_t toNSec() const
dai::ros::getFrameTime
::ros::Time getFrameTime(::ros::Time rosBaseTime, std::chrono::time_point< std::chrono::steady_clock > steadyBaseTime, std::chrono::time_point< std::chrono::steady_clock, std::chrono::steady_clock::duration > currTimePoint)
Definition: depthaiUtility.hpp:104
dai::ros::lerp
T lerp(const T &a, const T &b, const double t)
Definition: depthaiUtility.hpp:117

depthai_bridge
Author(s): Sachin Guruswamy
autogenerated on Tue May 10 2022 03:01:27