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

fix(ros): include publishOther and reduce the range

Browse Source
This commit is contained in:
TinyOh 2018-12-29 09:53:25 +08:00
parent 812638b7dc
commit ceec4f492a
1 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -439,9 +439,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 +459,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 +534,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);