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

Merge branch 'hotfix/2.2.4'

Browse Source
This commit is contained in:
John Zhao 2018-12-31 15:40:27 +08:00
commit 203c5ce18b
1 changed files with 7 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -51,7 +51,6 @@ inline double compute_time(const double end, const double start) {
class ROSWrapperNodelet : public nodelet::Nodelet {
public:
ROSWrapperNodelet() {
pthread_mutex_init(&mutex_data, nullptr);
}
~ROSWrapperNodelet() {
@ -112,7 +111,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
return now == pre;
}
inline bool is_annormal(std::uint32_t now,
inline bool is_abnormal(std::uint32_t now,
std::uint32_t pre) {
static std::uint64_t unit =
std::numeric_limits<std::uint32_t>::max();
@ -129,11 +128,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
if (is_overflow(_hard_time, hard_time_now[stream])) {
acc[stream]++;
} else if (is_repeated(_hard_time, hard_time_now[stream])) {
NODELET_INFO_STREAM("WARNING:: Image time stamp is repeated.");
} else if (is_annormal(_hard_time, hard_time_now[stream])) {
NODELET_INFO_STREAM("WARNING:: Image time stamp is annormal.");
}
hard_time_now[stream] = _hard_time;
return hardTimeToSoftTime(
@ -147,11 +143,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
if (is_overflow(_hard_time, hard_time_now)) {
acc++;
} else if (is_repeated(_hard_time, hard_time_now)) {
NODELET_INFO_STREAM("WARNING:: Imu time stamp is repeated.");
} else if (is_annormal(_hard_time, hard_time_now)) {
NODELET_INFO_STREAM("WARNING:: Imu time stamp is annormal.");
}
hard_time_now = _hard_time;
return hardTimeToSoftTime(
@ -162,6 +155,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
initDevice();
NODELET_FATAL_COND(api_ == nullptr, "No MYNT EYE device selected :(");
pthread_mutex_init(&mutex_data_, nullptr);
nh_ = getMTNodeHandle();
private_nh_ = getMTPrivateNodeHandle();
@ -439,9 +433,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// img_time_prev = data.img->timestamp;
++left_count_;
pthread_mutex_lock(&mutex_data);
publishCamera(Stream::LEFT, data, left_count_, stamp);
pthread_mutex_unlock(&mutex_data);
NODELET_DEBUG_STREAM(
Stream::LEFT << ", count: " << left_count_
<< ", frame_id: " << data.img->frame_id
@ -461,9 +453,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
data.img->timestamp, Stream::RIGHT);
++right_count_;
pthread_mutex_lock(&mutex_data);
publishCamera(Stream::RIGHT, data, right_count_, stamp);
pthread_mutex_unlock(&mutex_data);
NODELET_DEBUG_STREAM(
Stream::RIGHT
<< ", count: " << right_count_ << ", frame_id: "
@ -538,12 +528,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
header.seq = seq;
header.stamp = stamp;
header.frame_id = frame_ids_[stream];
pthread_mutex_lock(&mutex_data_);
cv::Mat img = data.frame;
if (stream == Stream::DISPARITY) { // 32FC1 > 8UC1 = MONO8
img.convertTo(img, CV_8UC1);
}
auto &&msg =
cv_bridge::CvImage(header, camera_encodings_[stream], img).toImageMsg();
pthread_mutex_unlock(&mutex_data_);
auto &&info = getCameraInfo(stream);
info->header.stamp = msg->header.stamp;
camera_publishers_[stream].publish(msg, info);
@ -978,7 +970,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
pthread_mutex_t mutex_data;
pthread_mutex_t mutex_data_;
// camera:
// LEFT, RIGHT, LEFT_RECTIFIED, RIGHT_RECTIFIED,