i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 20:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed multi camera namespace error

Browse Source
This commit is contained in:
Joe Dong
2023-02-16 16:14:38 +08:00
parent 31c0f91372
commit 689e6b22c7
34 changed files with 111 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+24 -4
View File
@@ -96,7 +96,10 @@ void OBCameraNode::setupProfiles() {
for (const auto& elem : IMAGE_STREAMS) {
if (enable_stream_[elem]) {
const auto& sensor = sensors_[elem];
CHECK_NOTNULL(sensor.get());
auto profiles = sensor->getStreamProfileList();
CHECK_NOTNULL(profiles.get());
CHECK(profiles->count() > 0);
for (size_t i = 0; i < profiles->count(); i++) {
auto profile = profiles->getProfile(i)->as<ob::VideoStreamProfile>();
RCLCPP_DEBUG_STREAM(
@@ -106,11 +109,23 @@ void OBCameraNode::setupProfiles() {
<< ", Height: " << profile->height() << ", FPS: " << profile->fps());
soupported_profiles_[elem].emplace_back(profile);
}
std::shared_ptr<ob::VideoStreamProfile> selected_profile;
std::shared_ptr<ob::VideoStreamProfile> default_profile;
try {
selected_profile =
profiles->getVideoStreamProfile(width_[elem], height_[elem], format_[elem], fps_[elem]);
default_profile =
profiles->getVideoStreamProfile(width_[elem], height_[elem], format_[elem]);
} catch (const ob::Error& ex) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get profile: " << ex.getMessage());
RCLCPP_ERROR_STREAM(
logger_, "Stream: " << magic_enum::enum_name(elem.first)
<< ", Stream Index: " << elem.second << ", Width: " << width_[elem]
<< ", Height: " << height_[elem] << ", FPS: " << fps_[elem]
<< ", Format: " << magic_enum::enum_name(format_[elem]));
throw;
}
auto selected_profile =
profiles->getVideoStreamProfile(width_[elem], height_[elem], format_[elem], fps_[elem]);
auto default_profile =
profiles->getVideoStreamProfile(width_[elem], height_[elem], format_[elem]);
if (!selected_profile) {
RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! "
<< " Stream: " << magic_enum::enum_name(elem.first)
@@ -261,6 +276,11 @@ void OBCameraNode::setupPipelineConfig() {
}
for (const auto& stream_index : IMAGE_STREAMS) {
if (enable_stream_[stream_index]) {
RCLCPP_INFO_STREAM(logger_, "Enable " << stream_name_[stream_index] << " stream");
RCLCPP_INFO_STREAM(
logger_, "Stream " << stream_name_[stream_index] << " width: " << width_[stream_index]
<< " height: " << height_[stream_index] << " fps: "
<< fps_[stream_index] << " format: " << format_str_[stream_index]);
pipeline_config_->enableStream(stream_profile_[stream_index]);
}
}
orbbec_camera/src/ob_camera_node_factory.cpp
+29 -20
View File
@@ -136,7 +136,6 @@ void OBCameraNodeFactory::queryDevice() {
}
onDeviceConnected(device_list);
} else {
RCLCPP_INFO_STREAM(logger_, "Device connected");
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
}
@@ -191,29 +190,39 @@ void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &lis
RCLCPP_INFO_STREAM(logger_, "Failed to wait semaphore " << strerror(errno));
return;
}
try {
auto device = list->getDeviceBySN(serial_number_.c_str());
if (device == nullptr) {
for (size_t i = 0; i < list->deviceCount(); i++) {
for (size_t i = 0; i < list->deviceCount(); i++) {
try {
auto pid = list->pid(i);
if ((pid >= OPENNI_START_PID && pid <= OPENNI_END_PID) || pid == ASTRA_MINI_PID ||
pid == ASTRA_MINI_S_PID) {
// openNI device
auto dev = list->getDevice(i);
if (dev != nullptr) {
std::string sn = dev->getDeviceInfo()->serialNumber();
RCLCPP_INFO_STREAM(logger_, "Device serial number: " << sn);
if (sn == serial_number_ || lower_sn == sn) {
RCLCPP_INFO_STREAM(logger_, "Device serial number matched: " << sn);
device = dev;
break;
}
auto device_info = dev->getDeviceInfo();
if (device_info->serialNumber() == serial_number_) {
RCLCPP_INFO_STREAM(
logger_, "Device serial number " << device_info->serialNumber() << " matched");
device_ = dev;
break;
}
} else {
std::string sn = list->serialNumber(i);
RCLCPP_INFO_STREAM(logger_, "Device serial number: " << sn);
if (sn == serial_number_) {
RCLCPP_INFO_STREAM(logger_, "Device serial number <<" << sn << " matched");
auto dev = list->getDevice(i);
device_ = dev;
break;
}
}
} catch (ob::Error &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.getMessage());
} catch (std::exception &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.what());
} catch (...) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info");
}
device_ = device;
} catch (ob::Error &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.getMessage());
} catch (std::exception &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info " << e.what());
} catch (...) {
RCLCPP_INFO_STREAM(logger_, "Failed to get device info");
}
if (device_ == nullptr) {