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

Merge branch 'hotfix/2.2.4' into develop

Browse Source 
# Conflicts:
#	wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
This commit is contained in:
John Zhao 2018-12-31 15:53:06 +08:00
commit 667c53ae6d
1 changed files with 22 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
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() {
@ -119,8 +118,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
return now == pre; return now == pre;
} }
inline bool is_annormal(std::uint64_t now, inline bool is_abnormal(std::uint32_t now,
std::uint64_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();
@ -136,11 +135,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(
@ -155,13 +151,7 @@ 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(
@ -175,6 +165,8 @@ 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);
// node params // node params
std::map<Stream, std::string> stream_names{ std::map<Stream, std::string> stream_names{
@ -493,16 +485,13 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// : (data.img->timestamp - img_time_prev) * 0.01f) << " // : (data.img->timestamp - img_time_prev) * 0.01f) << "
// ms"); // ms");
// img_time_prev = data.img->timestamp; // img_time_prev = data.img->timestamp;
pthread_mutex_lock(&mutex_data);
publishCamera(Stream::LEFT, data, left_count_, stamp); publishCamera(Stream::LEFT, data, left_count_, stamp);
pthread_mutex_unlock(&mutex_data);
publishMono(Stream::LEFT, data, left_count_, stamp); publishMono(Stream::LEFT, data, left_count_, stamp);
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
<< ", timestamp: " << data.img->timestamp << ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << ", exposure_time: " << data.img->exposure_time);
<< data.img->exposure_time);
} }
}); });
left_time_beg_ = ros::Time::now().toSec(); left_time_beg_ = ros::Time::now().toSec();
@ -516,18 +505,16 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
Stream::RIGHT, [&](const api::StreamData &data) { Stream::RIGHT, [&](const api::StreamData &data) {
++right_count_; ++right_count_;
if (right_count_ > 10) { if (right_count_ > 10) {
// 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);
pthread_mutex_lock(&mutex_data); publishCamera(Stream::RIGHT, data, right_count_, stamp);
publishCamera(Stream::RIGHT, data, right_count_, stamp); publishMono(Stream::RIGHT, data, right_count_, stamp);
pthread_mutex_unlock(&mutex_data); NODELET_DEBUG_STREAM(
publishMono(Stream::RIGHT, data, right_count_, stamp); Stream::RIGHT << ", count: " << right_count_
NODELET_DEBUG_STREAM( << ", frame_id: " << data.img->frame_id
Stream::RIGHT << ", timestamp: " << data.img->timestamp
<< ", count: " << right_count_ << ", frame_id: " << ", exposure_time: " << data.img->exposure_time);
<< data.img->frame_id << ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time);
} }
}); });
right_time_beg_ = ros::Time::now().toSec(); right_time_beg_ = ros::Time::now().toSec();
@ -614,12 +601,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);
@ -1165,7 +1154,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_;
Model model_; Model model_;
std::map<Option, std::string> option_names_; std::map<Option, std::string> option_names_;