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

fix(ros): record -a core error

Browse Source
This commit is contained in:
TinyOh 2018-12-28 17:49:59 +08:00
parent 6e638813f2
commit 812638b7dc
1 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -50,7 +50,9 @@ 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() {
// std::cout << __func__ << std::endl; // std::cout << __func__ << std::endl;
@ -420,7 +422,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
if (camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 && if (camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 &&
!is_published_[Stream::LEFT]) { !is_published_[Stream::LEFT]) {
api_->SetStreamCallback( api_->SetStreamCallback(
Stream::LEFT, [this](const api::StreamData &data) { Stream::LEFT, [&](const api::StreamData &data) {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp( ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, Stream::LEFT); data.img->timestamp, Stream::LEFT);
@ -437,7 +439,9 @@ 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
@ -451,13 +455,15 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
if (camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 && if (camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 &&
!is_published_[Stream::RIGHT]) { !is_published_[Stream::RIGHT]) {
api_->SetStreamCallback( api_->SetStreamCallback(
Stream::RIGHT, [this](const api::StreamData &data) { Stream::RIGHT, [&](const api::StreamData &data) {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp( ros::Time stamp = checkUpTimeStamp(
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: "
@ -972,6 +978,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
ros::NodeHandle nh_; ros::NodeHandle nh_;
ros::NodeHandle private_nh_; ros::NodeHandle private_nh_;
pthread_mutex_t mutex_data;
// camera: // camera:
// LEFT, RIGHT, LEFT_RECTIFIED, RIGHT_RECTIFIED, // LEFT, RIGHT, LEFT_RECTIFIED, RIGHT_RECTIFIED,
// DISPARITY, DISPARITY_NORMALIZED, // DISPARITY, DISPARITY_NORMALIZED,