From ceec4f492a26bf56bb4898d3b691e81aa1fcbe52 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Sat, 29 Dec 2018 09:53:25 +0800 Subject: [PATCH 1/4] fix(ros): include publishOther and reduce the range --- .../ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) 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 d03ed32..56284d6 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 @@ -439,9 +439,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 +459,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 +534,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); From 87c3c81a937635cc9b1dd3a36928aacdcb61b361 Mon Sep 17 00:00:00 2001 From: kalman Date: Sat, 29 Dec 2018 16:32:03 +0800 Subject: [PATCH 2/4] fix(ros):delete timestamp repeated/abnormal jundge --- .../src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) 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 56284d6..8199f14 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 @@ -129,11 +129,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 +144,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( From e34e6e1466a4d3dec905c608532cfcd6db089ccb Mon Sep 17 00:00:00 2001 From: kalman Date: Sat, 29 Dec 2018 16:40:03 +0800 Subject: [PATCH 3/4] style(ros):fix spell mistake --- wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 8199f14..8ef3fcb 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 @@ -112,7 +112,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::max(); From a3206e5be5ff21b520b5dc5f683965684e1e1bec Mon Sep 17 00:00:00 2001 From: kalman Date: Sat, 29 Dec 2018 16:47:18 +0800 Subject: [PATCH 4/4] style(ros):rename mutex_data --- .../ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 8ef3fcb..c05c3e0 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 @@ -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() { @@ -156,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(); @@ -528,14 +528,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet { header.seq = seq; header.stamp = stamp; header.frame_id = frame_ids_[stream]; - pthread_mutex_lock(&mutex_data); + 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); + pthread_mutex_unlock(&mutex_data_); auto &&info = getCameraInfo(stream); info->header.stamp = msg->header.stamp; camera_publishers_[stream].publish(msg, info); @@ -970,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,