tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add imu publish by sync

Browse Source
This commit is contained in:
Kalman 2018-10-23 16:08:59 +08:00
parent 77e4411a4c
commit c58dd28f63
3 changed files with 81 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/mynteye/types.h
View File

@ -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;

2
wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
View File

@ -37,7 +37,7 @@
<arg name="enable_disparity" default="false" />
<arg name="enable_disparity_norm" default="false" />
<arg name="enable_points" default="false" />
<arg name="enable_depth" default="true" />
<arg name="enable_depth" default="false" />
<!-- device options, -1 will not set the value -->

81
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -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<ImuData> imu_accel_;
std::shared_ptr<ImuData> imu_gyro_;
bool publish_imu_by_sync_ = false;
};
MYNTEYE_END_NAMESPACE