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