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