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

feat(ros): optimized multiple device process for ros

Browse Source
This commit is contained in:
Osenberg 2019-03-23 14:23:23 +08:00
parent 042293fb3b
commit 415cf31a36
2 changed files with 28 additions and 49 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_multiple.launch
View File

@ -2,6 +2,4 @@
<launch>
<include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_1.launch" />
<include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_2.launch" />
<!-- <include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_3.launch" />
<include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_4.launch" /> -->
</launch>

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

@ -1098,6 +1098,18 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
Context context;
auto &&devices = context.devices();
size_t n = devices.size();
NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :(");
NODELET_INFO_STREAM("MYNT EYE devices:");
for (size_t i = 0; i < n; i++) {
auto &&device = devices[i];
auto &&name = device->GetInfo(Info::DEVICE_NAME);
auto &&serial_number = device->GetInfo(Info::SERIAL_NUMBER);
NODELET_INFO_STREAM(" index: " << i << ", name: " <<
name << ", serial number: " << serial_number);
}
bool is_multiple = false;
private_nh_.getParam("is_multiple", is_multiple);
if (is_multiple) {
@ -1106,48 +1118,16 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
NODELET_FATAL_COND(sn.empty(), "Must set serial_number "
"in mynteye_1.launch and mynteye_2.launch.");
size_t n = devices.size();
NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :(");
NODELET_INFO_STREAM("MYNT EYE devices:");
for (size_t i = 0; i < n; i++) {
auto &&device = devices[i];
auto &&name = device->GetInfo(Info::DEVICE_NAME);
auto &&serial_number = device->GetInfo(Info::SERIAL_NUMBER);
NODELET_INFO_STREAM(" index: " << i << ", name: " <<
name << ", serial number: " << serial_number);
}
for (size_t i = 0; i < n; i++) {
auto &&device = devices[i];
auto &&name = device->GetInfo(Info::DEVICE_NAME);
auto &&serial_number = device->GetInfo(Info::SERIAL_NUMBER);
NODELET_INFO_STREAM(" index: " << i << ", name: " <<
name << ", serial number: " << serial_number);
if (sn == serial_number)
return device;
#if 0
if (i == (n - 1)) {
/*
NODELET_FATAL_COND(i == (n - 1), "No corresponding device was found,"
" check the serial_number configuration. ");
*/
NODELET_FATAL_COND(i == (n - 1), "No corresponding device was found,"
" check the serial_number configuration. ");
return nullptr;
}
#endif
}
}
size_t n = devices.size();
NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :(");
NODELET_INFO_STREAM("MYNT EYE devices:");
for (size_t i = 0; i < n; i++) {
auto &&device = devices[i];
auto &&name = device->GetInfo(Info::DEVICE_NAME);
NODELET_INFO_STREAM(" index: " << i << ", name: " << name);
}
} else {
if (n <= 1) {
NODELET_INFO_STREAM("Only one MYNT EYE device, select index: 0");
return devices[0];
@ -1164,6 +1144,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
return devices[i];
}
}
}
return nullptr;
}