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

Temporary version

Browse Source
This commit is contained in:
Osenberg-Y 2018-08-28 16:46:51 +08:00
parent d4343b574a
commit 692cbac8d8
2 changed files with 90 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
View File

@ -38,7 +38,7 @@
<arg name="enable_disparity" default="false" />
<arg name="enable_disparity_norm" default="false" />
<arg name="enable_points" default="false" />
<arg name="enable_depth" default="true" />
<arg name="enable_depth" default="false" />
<!-- device options, -1 will not set the value -->

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

@ -173,6 +173,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}
NODELET_INFO_STREAM(it->first << ": " << api_->GetOptionValue(it->first));
}
frame_rate_ = api_->GetOptionValue(Option::FRAME_RATE);
// publishers
@ -227,8 +228,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
NODELET_INFO_STREAM("Advertized service " << DEVICE_INFO_SERVICE);
publishStaticTransforms();
ros::Rate loop_rate(frame_rate_);
while (private_nh_.ok()) {
publishTopics();
loop_rate.sleep();
}
}
@ -268,6 +271,84 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
return true;
}
void SetIsPublished(const Stream &stream) {
is_published_[stream] = false;
switch (stream) {
case Stream::LEFT_RECTIFIED: {
if (is_published_[Stream::RIGHT_RECTIFIED]) {
SetIsPublished(Stream::RIGHT_RECTIFIED);
}
if (is_published_[Stream::DISPARITY]) {
SetIsPublished(Stream::DISPARITY);
}
} break;
case Stream::RIGHT_RECTIFIED: {
if (is_published_[Stream::LEFT_RECTIFIED]) {
SetIsPublished(Stream::RIGHT_RECTIFIED);
}
if (is_published_[Stream::DISPARITY]) {
SetIsPublished(Stream::DISPARITY);
}
} break;
case Stream::DISPARITY: {
if (is_published_[Stream::DISPARITY_NORMALIZED]) {
SetIsPublished(Stream::DISPARITY_NORMALIZED);
}
if (is_published_[Stream::POINTS]) {
SetIsPublished(Stream::POINTS);
}
} break;
case Stream::DISPARITY_NORMALIZED: {
} break;
case Stream::POINTS: {
if (is_published_[Stream::DEPTH]) {
SetIsPublished(Stream::DEPTH);
}
} break;
case Stream::DEPTH: {
} break;
default:
return;
}
}
void publishPoint(const Stream &stream) {
auto &&points_num = points_publisher_.getNumSubscribers();
if (points_num == 0 && is_published_[stream]) {
SetIsPublished(stream);
api_->DisableStreamData(stream);
} else if (points_num > 0 && !is_published_[Stream::POINTS]) {
api_->EnableStreamData(Stream::POINTS);
api_->SetStreamCallback(
Stream::POINTS, [this](const api::StreamData &data) {
static std::size_t count = 0;
++count;
publishPoints(data, count, ros::Time::now());
});
is_published_[Stream::POINTS] = true;
}
}
void publishOthers(const Stream &stream) {
auto stream_num = camera_publishers_[stream].getNumSubscribers();
if (stream_num == 0 && is_published_[stream]) {
// Stop computing when was not subcribed
SetIsPublished(stream);
api_->DisableStreamData(stream);
} else if (stream_num > 0 && !is_published_[stream]) {
// Start computing and publishing when was subcribed
api_->EnableStreamData(stream);
api_->SetStreamCallback(
stream, [this, stream](const api::StreamData &data) {
// data.img is null, not hard timestamp
static std::size_t count = 0;
++count;
publishCamera(stream, data, count, ros::Time::now());
});
is_published_[stream] = true;
}
}
void publishTopics() {
if (camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 &&
!is_published_[Stream::LEFT]) {
@ -317,33 +398,16 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}
std::vector<Stream> other_streams{
Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, Stream::DISPARITY,
Stream::DISPARITY_NORMALIZED, Stream::DEPTH};
Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED,
Stream::DISPARITY, Stream::DISPARITY_NORMALIZED,
Stream::POINTS, Stream::DEPTH};
for (auto &&stream : other_streams) {
if (camera_publishers_[stream].getNumSubscribers() == 0 &&
is_published_[stream]) {
continue;
if (stream != Stream::POINTS) {
publishOthers(stream);
} else {
publishPoint(stream);
}
api_->SetStreamCallback(
stream, [this, stream](const api::StreamData &data) {
// data.img is null, not hard timestamp
static std::size_t count = 0;
++count;
publishCamera(stream, data, count, ros::Time::now());
});
is_published_[stream] = true;
}
if (points_publisher_.getNumSubscribers() > 0 &&
!is_published_[Stream::POINTS]) {
api_->SetStreamCallback(
Stream::POINTS, [this](const api::StreamData &data) {
static std::size_t count = 0;
++count;
publishPoints(data, count, ros::Time::now());
});
is_published_[Stream::POINTS] = true;
}
if (!is_motion_published_) {
@ -882,6 +946,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
std::map<Stream, bool> is_published_;
bool is_motion_published_;
bool is_started_;
int frame_rate_;
};
MYNTEYE_END_NAMESPACE