ImuConverter.cpp
Go to the documentation of this file.
1 
2 #include <depthai_bridge/ImuConverter.hpp>
3 
4 namespace dai {
5 
6 namespace ros {
7 
8 ImuConverter::ImuConverter(const std::string& frameName, ImuSyncMethod syncMode, double linear_accel_cov, double angular_velocity_cov)
9  : _frameName(frameName),
10  _syncMode(syncMode),
11  _linear_accel_cov(linear_accel_cov),
12  _angular_velocity_cov(angular_velocity_cov),
13  _sequenceNum(0),
14  _steadyBaseTime(std::chrono::steady_clock::now()) {
15 #ifdef IS_ROS2
16  _rosBaseTime = rclcpp::Clock().now();
17 #else
18  _rosBaseTime = ::ros::Time::now();
19 #endif
20 }
21 
22 void ImuConverter::FillImuData_LinearInterpolation(std::vector<IMUPacket>& imuPackets, std::deque<ImuMsgs::Imu>& imuMsgs) {
23  // int accelSequenceNum = -1, gyroSequenceNum = -1;
24  static std::deque<dai::IMUReportAccelerometer> accelHist;
25  static std::deque<dai::IMUReportGyroscope> gyroHist;
26  // std::deque<dai::IMUReportRotationVectorWAcc> rotationVecHist;
27 
28  for(int i = 0; i < imuPackets.size(); ++i) {
29  if(accelHist.size() == 0) {
30  accelHist.push_back(imuPackets[i].acceleroMeter);
31  } else if(accelHist.back().sequence != imuPackets[i].acceleroMeter.sequence) {
32  accelHist.push_back(imuPackets[i].acceleroMeter);
33  }
34 
35  if(gyroHist.size() == 0) {
36  gyroHist.push_back(imuPackets[i].gyroscope);
37  } else if(gyroHist.back().sequence != imuPackets[i].gyroscope.sequence) {
38  gyroHist.push_back(imuPackets[i].gyroscope);
39  }
40 
41  if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) {
42  if(accelHist.size() < 3) {
43  continue;
44  } else {
45  dai::IMUReportAccelerometer accel0, accel1;
46  dai::IMUReportGyroscope currGyro;
47  accel0.sequence = -1;
48  DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", " Interpolating LINEAR_INTERPOLATE_ACCEL mode " << std::endl);
49  while(accelHist.size()) {
50  if(accel0.sequence == -1) {
51  accel0 = accelHist.front();
52  accelHist.pop_front();
53  } else {
54  accel1 = accelHist.front();
55  accelHist.pop_front();
56  // auto dt = 1e-9
57  // * static_cast<double>(
58  // std::chrono::duration_cast<std::chrono::nanoseconds>(accel1.timestamp.get() - accel0.timestamp.get()).count());
59 
60  // remove std::milli to get in seconds
61  std::chrono::duration<double, std::milli> duration_ms = accel1.timestamp.get() - accel0.timestamp.get();
62  double dt = duration_ms.count();
63 
64  if(!gyroHist.size()) {
65  DEPTHAI_ROS_WARN_STREAM("IMU INTERPOLATION: ", "Gyro data not found. Dropping accel data points");
66  }
67  while(gyroHist.size()) {
68  currGyro = gyroHist.front();
69 
70  DEPTHAI_ROS_DEBUG_STREAM(
71  "IMU INTERPOLATION: ",
72  "Accel 0: Seq => " << accel0.sequence << " timeStamp => " << (accel0.timestamp.get() - _steadyBaseTime).count() << std::endl);
73  DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ",
74  "currGyro 0: Seq => " << currGyro.sequence << "timeStamp => "
75  << (currGyro.timestamp.get() - _steadyBaseTime).count() << std::endl);
76  DEPTHAI_ROS_DEBUG_STREAM(
77  "IMU INTERPOLATION: ",
78  "Accel 1: Seq => " << accel1.sequence << " timeStamp => " << (accel1.timestamp.get() - _steadyBaseTime).count() << std::endl);
79  if(currGyro.timestamp.get() > accel0.timestamp.get() && currGyro.timestamp.get() <= accel1.timestamp.get()) {
80  // auto alpha = std::chrono::duration_cast<std::chrono::nanoseconds>(currGyro.timestamp.get() - accel0.timestamp.get()).count();
81  // / dt;
82  // remove std::milli to get in seconds
83  std::chrono::duration<double, std::milli> diff = currGyro.timestamp.get() - accel0.timestamp.get();
84  const double alpha = diff.count() / dt;
85  dai::IMUReportAccelerometer interpAccel = lerpImu(accel0, accel1, alpha);
86  imuMsgs.push_back(CreateUnitMessage(interpAccel, currGyro));
87  gyroHist.pop_front();
88  } else if(currGyro.timestamp.get() > accel1.timestamp.get()) {
89  accel0 = accel1;
90  if(accelHist.size()) {
91  accel1 = accelHist.front();
92  accelHist.pop_front();
93  duration_ms = accel1.timestamp.get() - accel0.timestamp.get();
94  dt = duration_ms.count();
95  } else {
96  break;
97  }
98  } else {
99  gyroHist.pop_front();
100  DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", "Droppinh GYRO with old timestamps which are below accel10 \n");
101  }
102  }
103  // gyroHist.push_back(currGyro); // Undecided whether this is necessary
104  accel0 = accel1;
105  }
106  }
107  DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", "Count ->" << i << " Placing Accel 0 Seq Number :" << accel0.sequence << std::endl);
108 
109  accelHist.push_back(accel0);
110  }
111  } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) {
112  if(gyroHist.size() < 3) {
113  continue;
114  } else {
115  dai::IMUReportGyroscope gyro0, gyro1;
116  dai::IMUReportAccelerometer currAccel;
117  gyro0.sequence = -1;
118  DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", " Interpolating LINEAR_INTERPOLATE_GYRO mode " << std::endl);
119  while(gyroHist.size()) {
120  if(gyro0.sequence == -1) {
121  gyro0 = gyroHist.front();
122  gyroHist.pop_front();
123  } else {
124  gyro1 = gyroHist.front();
125  gyroHist.pop_front();
126  // remove std::milli to get in seconds
127  std::chrono::duration<double, std::milli> duration_ms = gyro1.timestamp.get() - gyro0.timestamp.get();
128  double dt = duration_ms.count();
129 
130  if(!accelHist.size()) {
131  DEPTHAI_ROS_WARN_STREAM("IMU INTERPOLATION: ", "Accel data not found. Dropping data");
132  }
133  while(accelHist.size()) {
134  currAccel = accelHist.front();
135  DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ",
136  "gyro 0: Seq => " << gyro0.sequence << std::endl
137  << " timeStamp => " << (gyro0.timestamp.get() - _steadyBaseTime).count()
138  << std::endl);
139  DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ",
140  "currAccel 0: Seq => " << currAccel.sequence << std::endl
141  << " timeStamp => " << (currAccel.timestamp.get() - _steadyBaseTime).count()
142  << std::endl);
143  DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ",
144  "gyro 1: Seq => " << gyro1.sequence << std::endl
145  << " timeStamp => " << (gyro1.timestamp.get() - _steadyBaseTime).count()
146  << std::endl);
147  if(currAccel.timestamp.get() > gyro0.timestamp.get() && currAccel.timestamp.get() <= gyro1.timestamp.get()) {
148  // remove std::milli to get in seconds
149  std::chrono::duration<double, std::milli> diff = currAccel.timestamp.get() - gyro0.timestamp.get();
150  const double alpha = diff.count() / dt;
151  dai::IMUReportGyroscope interpGyro = lerpImu(gyro0, gyro1, alpha);
152  imuMsgs.push_back(CreateUnitMessage(currAccel, interpGyro));
153  accelHist.pop_front();
154  } else if(currAccel.timestamp.get() > gyro1.timestamp.get()) {
155  gyro0 = gyro1;
156  if(gyroHist.size()) {
157  gyro1 = gyroHist.front();
158  gyroHist.pop_front();
159  duration_ms = gyro1.timestamp.get() - gyro0.timestamp.get();
160  dt = duration_ms.count();
161  } else {
162  break;
163  }
164  } else {
165  accelHist.pop_front();
166  DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", "Droppinh ACCEL with old timestamps which are below accel10 \n");
167  }
168  }
169  gyro0 = gyro1;
170  }
171  }
172  gyroHist.push_back(gyro0);
173  }
174  }
175  }
176 }
177 
178 ImuMsgs::Imu ImuConverter::CreateUnitMessage(dai::IMUReportAccelerometer accel, dai::IMUReportGyroscope gyro) {
179  ImuMsgs::Imu interpMsg;
180  interpMsg.linear_acceleration.x = accel.x;
181  interpMsg.linear_acceleration.y = accel.y;
182  interpMsg.linear_acceleration.z = accel.z;
183 
184  interpMsg.angular_velocity.x = gyro.x;
185  interpMsg.angular_velocity.y = gyro.y;
186  interpMsg.angular_velocity.z = gyro.z;
187 
188  interpMsg.orientation.x = 0.0;
189  interpMsg.orientation.y = 0.0;
190  interpMsg.orientation.z = 0.0;
191  interpMsg.orientation.w = 0.0;
192 
193  interpMsg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
194  interpMsg.linear_acceleration_covariance = {_linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov};
195  interpMsg.angular_velocity_covariance = {_angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov};
196 
197  interpMsg.header.frame_id = _frameName;
198 #ifndef IS_ROS2
199  interpMsg.header.seq = _sequenceNum;
200  _sequenceNum++;
201 #endif
202 
203  if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) {
204  interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, gyro.timestamp.get());
205  } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) {
206  interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, accel.timestamp.get());
207  } else {
208  interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, accel.timestamp.get());
209  }
210 
211  return interpMsg;
212 }
213 
214 void ImuConverter::toRosMsg(std::shared_ptr<dai::IMUData> inData, std::deque<ImuMsgs::Imu>& outImuMsgs) {
215  if(_syncMode != ImuSyncMethod::COPY) {
216  FillImuData_LinearInterpolation(inData->packets, outImuMsgs);
217  } else {
218  for(int i = 0; i < inData->packets.size(); ++i) {
219  auto accel = inData->packets[i].acceleroMeter;
220  auto gyro = inData->packets[i].gyroscope;
221  outImuMsgs.push_back(CreateUnitMessage(accel, gyro));
222  }
223  }
224 }
225 
226 } // namespace ros
227 } // namespace dai
dai::ros::ImuConverter::FillImuData_LinearInterpolation
void FillImuData_LinearInterpolation(std::vector< IMUPacket > &imuPackets, std::deque< ImuMsgs::Imu > &imuMsgs)
Definition: ImuConverter.cpp:22
dai::ros::ImuConverter::_frameName
const std::string _frameName
Definition: ImuConverter.hpp:50
dai::ros::ImuConverter::toRosMsg
void toRosMsg(std::shared_ptr< dai::IMUData > inData, std::deque< ImuMsgs::Imu > &outImuMsg)
Definition: ImuConverter.cpp:214
dai::ros::lerpImu
T lerpImu(const T &a, const T &b, const double t)
Definition: depthaiUtility.hpp:122
dai::ros::ImuConverter::ImuConverter
ImuConverter(const std::string &frameName, ImuSyncMethod syncMode=ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL, double linear_accel_cov=0.0, double angular_velocity_cov=0.0)
Definition: ImuConverter.cpp:8
DEPTHAI_ROS_DEBUG_STREAM
#define DEPTHAI_ROS_DEBUG_STREAM(loggerName, args)
Definition: depthaiUtility.hpp:67
diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
dai::ros::ImuConverter::CreateUnitMessage
ImuMsgs::Imu CreateUnitMessage(dai::IMUReportAccelerometer accel, dai::IMUReportGyroscope gyro)
Definition: ImuConverter.cpp:178
DEPTHAI_ROS_WARN_STREAM
#define DEPTHAI_ROS_WARN_STREAM(loggerName, args)
Definition: depthaiUtility.hpp:77
dai::ros::ImuConverter::_steadyBaseTime
std::chrono::time_point< std::chrono::steady_clock > _steadyBaseTime
Definition: ImuConverter.hpp:52
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::ImuConverter::_syncMode
ImuSyncMethod _syncMode
Definition: ImuConverter.hpp:51
ros::Time::now
static Time now()

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