diff --git a/include/mynteye/types.h b/include/mynteye/types.h
index 633e11f..66fcd33 100644
--- a/include/mynteye/types.h
+++ b/include/mynteye/types.h
@@ -525,7 +525,7 @@ std::ostream &operator<<(std::ostream &os, const Extrinsics &ex);
struct MYNTEYE_API ImgData {
/** Image frame id */
std::uint16_t frame_id;
- /** Image timestamp in 0.01ms */
+ /** Image timestamp in 1us */
std::uint64_t timestamp;
/** Image exposure time, virtual value in [1, 480] */
std::uint16_t exposure_time;
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
index c906c32..6b8f792 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
@@ -37,7 +37,7 @@
-
+
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
index 9bb7b8c..8b42ebc 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
@@ -62,8 +62,13 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
<< ", fps: " << (left_count_ / time_elapsed);
LOG(INFO) << "Right count: " << right_count_
<< ", fps: " << (right_count_ / time_elapsed);
- LOG(INFO) << "Imu count: " << imu_count_
- << ", hz: " << (imu_count_ / time_elapsed);
+ if (publish_imu_by_sync_) {
+ LOG(INFO) << "imu_sync_count: " << imu_sync_count_
+ << ", hz: " << (imu_sync_count_ / time_elapsed);
+ } else {
+ LOG(INFO) << "Imu count: " << imu_count_
+ << ", hz: " << (imu_count_ / time_elapsed);
+ }
// ROS messages could not be reliably printed here, using glog instead :(
// ros::Duration(1).sleep(); // 1s
@@ -322,6 +327,21 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// imu_time_prev = data.imu->timestamp;
++imu_count_;
+ if (publish_imu_by_sync_) {
+ if (data.imu) {
+ if (data.imu->flag == 1) { // accelerometer
+ imu_accel_ = data.imu;
+ publishImuBySync(stamp);
+ } else if (data.imu->flag == 2) { // gyroscope
+ imu_gyro_ = data.imu;
+ publishImuBySync(stamp);
+ } else {
+ NODELET_WARN_STREAM("Imu type is unknown");
+ }
+ } else {
+ NODELET_WARN_STREAM("Motion data is empty");
+ }
+ }
publishImu(data, imu_count_, stamp);
publishTemp(data.imu->temperature, imu_count_, stamp);
NODELET_DEBUG_STREAM(
@@ -484,6 +504,59 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
pub_imu_.publish(msg);
}
+ void publishImuBySync(ros::Time stamp) {
+ if (imu_accel_ == nullptr || imu_gyro_ == nullptr) {
+ return;
+ }
+ sensor_msgs::Imu msg;
+
+ msg.header.seq = imu_sync_count_;
+ msg.header.stamp = stamp;
+ msg.header.frame_id = imu_frame_id_;
+
+ // acceleration should be in m/s^2 (not in g's)
+ msg.linear_acceleration.x = imu_accel_->accel[0] * gravity_;
+ msg.linear_acceleration.y = imu_accel_->accel[1] * gravity_;
+ msg.linear_acceleration.z = imu_accel_->accel[2] * gravity_;
+
+ msg.linear_acceleration_covariance[0] = 0;
+ msg.linear_acceleration_covariance[1] = 0;
+ msg.linear_acceleration_covariance[2] = 0;
+
+ msg.linear_acceleration_covariance[3] = 0;
+ msg.linear_acceleration_covariance[4] = 0;
+ msg.linear_acceleration_covariance[5] = 0;
+
+ msg.linear_acceleration_covariance[6] = 0;
+ msg.linear_acceleration_covariance[7] = 0;
+ msg.linear_acceleration_covariance[8] = 0;
+
+ // velocity should be in rad/sec
+ msg.angular_velocity.x = imu_gyro_->gyro[0] * M_PI / 180;
+ msg.angular_velocity.y = imu_gyro_->gyro[1] * M_PI / 180;
+ msg.angular_velocity.z = imu_gyro_->gyro[2] * M_PI / 180;
+
+ msg.angular_velocity_covariance[0] = 0;
+ msg.angular_velocity_covariance[1] = 0;
+ msg.angular_velocity_covariance[2] = 0;
+
+ msg.angular_velocity_covariance[3] = 0;
+ msg.angular_velocity_covariance[4] = 0;
+ msg.angular_velocity_covariance[5] = 0;
+
+ msg.angular_velocity_covariance[6] = 0;
+ msg.angular_velocity_covariance[7] = 0;
+ msg.angular_velocity_covariance[8] = 0;
+
+ pub_imu_.publish(msg);
+
+ publishTemp(imu_accel_->temperature, imu_sync_count_, stamp);
+
+ ++imu_sync_count_;
+ imu_accel_ = nullptr;
+ imu_gyro_ = nullptr;
+ }
+
void publishTemp(float temperature, std::uint32_t seq, ros::Time stamp) {
if (pub_temp_.getNumSubscribers() == 0)
return;
@@ -795,6 +868,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
std::size_t left_count_ = 0;
std::size_t right_count_ = 0;
std::size_t imu_count_ = 0;
+ std::size_t imu_sync_count_ = 0;
+ std::shared_ptr imu_accel_;
+ std::shared_ptr imu_gyro_;
+ bool publish_imu_by_sync_ = false;
};
MYNTEYE_END_NAMESPACE