From 45765dd29c89e4c0a198c947e1e138a582351115 Mon Sep 17 00:00:00 2001 From: Joe Dong Date: Thu, 12 Oct 2023 20:52:10 +0800 Subject: [PATCH] fixed crash --- .../include/orbbec_camera/ob_camera_node.h | 32 ++++++++-------- orbbec_camera/launch/multi_camera.launch.py | 8 ++-- orbbec_camera/src/ob_camera_node.cpp | 4 +- orbbec_camera/src/ob_camera_node_driver.cpp | 37 +++++++++++-------- 4 files changed, 45 insertions(+), 36 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index df402ebb..95fa51ef 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright (c) 2023 Orbbec 3D Technology, Inc -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ + * Copyright (c) 2023 Orbbec 3D Technology, Inc + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *******************************************************************************/ #pragma once @@ -136,6 +136,8 @@ class OBCameraNode { void clean(); + void startStreams(); + private: struct IMUData { IMUData() = default; @@ -159,8 +161,6 @@ class OBCameraNode { void setupCameraCtrlServices(); - void startStreams(); - void startIMU(); void stopStreams(); diff --git a/orbbec_camera/launch/multi_camera.launch.py b/orbbec_camera/launch/multi_camera.launch.py index 35b08550..457f66f6 100644 --- a/orbbec_camera/launch/multi_camera.launch.py +++ b/orbbec_camera/launch/multi_camera.launch.py @@ -12,11 +12,11 @@ def generate_launch_description(): launch_file_dir = os.path.join(package_dir, 'launch') launch1_include = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(launch_file_dir, 'astra.launch.py') + os.path.join(launch_file_dir, 'dabai_dcw.launch.py') ), launch_arguments={ 'camera_name': 'camera_01', - 'usb_port': '5-3.4.4.3', + 'usb_port': '5-3.4.4.2.1', 'device_num': '2', 'sync_mode': 'free_run' }.items() @@ -24,11 +24,11 @@ def generate_launch_description(): launch2_include = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(launch_file_dir, 'astra.launch.py') + os.path.join(launch_file_dir, 'dabai_dcw.launch.py') ), launch_arguments={ 'camera_name': 'camera_02', - 'usb_port': '5-3.4.4.1', + 'usb_port': '5-3.4.3.1', 'device_num': '2', 'sync_mode': 'free_run' }.items() diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 9de7819b..c047c032 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -57,7 +57,6 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr devic #elif defined(USE_NV_HW_DECODER) jpeg_decoder_ = std::make_unique(width_[COLOR], height_[COLOR]); #endif - startStreams(); if (enable_d2c_viewer_) { auto rgb_qos = getRMWQosProfileFromString(image_qos_[COLOR]); auto depth_qos = getRMWQosProfileFromString(image_qos_[DEPTH]); @@ -252,6 +251,9 @@ void OBCameraNode::startStreams() { pipeline_->start(pipeline_config_, [this](const std::shared_ptr &frame_set) { onNewFrameSetCallback(frame_set); }); + } catch (...) { + RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline"); + throw std::runtime_error("Failed to start pipeline"); } if (enable_frame_sync_) { pipeline_->enableFrameSync(); diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index f92580bd..ea2c0e45 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -114,18 +114,7 @@ void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "device list count " << device_list->deviceCount()); if (!device_) { - try { - startDevice(device_list); - } catch (ob::Error &e) { - device_.reset(); - RCLCPP_ERROR_STREAM(logger_, "startDevice failed: " << e.getMessage()); - } catch (const std::exception &e) { - device_.reset(); - RCLCPP_ERROR_STREAM(logger_, "startDevice failed: " << e.what()); - } catch (...) { - device_.reset(); - RCLCPP_ERROR_STREAM(logger_, "startDevice failed"); - } + startDevice(device_list); } } @@ -183,7 +172,24 @@ void OBCameraNodeDriver::queryDevice() { std::this_thread::sleep_for(std::chrono::milliseconds(10)); continue; } - onDeviceConnected(device_list); + bool start_device_failed = false; + try { + onDeviceConnected(device_list); + } catch (ob::Error &e) { + RCLCPP_ERROR_STREAM(logger_, "Failed to start device " << e.getMessage()); + start_device_failed = true; + } catch (std::exception &e) { + RCLCPP_ERROR_STREAM(logger_, "Failed to start device " << e.what()); + start_device_failed = true; + } catch (...) { + RCLCPP_ERROR_STREAM(logger_, "Failed to start device"); + start_device_failed = true; + } + if (start_device_failed) { + std::unique_lock lock(reset_device_mutex_); + reset_device_flag_ = true; + reset_device_cond_.notify_all(); + } } else { std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } @@ -203,7 +209,7 @@ void OBCameraNodeDriver::resetDevice() { while (is_alive_ && rclcpp::ok()) { std::unique_lock lock(reset_device_mutex_); reset_device_cond_.wait(lock, - [this]() { return !is_alive_ || !rclcpp::ok() || reset_device_flag_; }); + [this]() { return !is_alive_ || !rclcpp::ok() || reset_device_flag_; }); if (!is_alive_ || !rclcpp::ok()) { break; } @@ -303,6 +309,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &dev ob_camera_node_.reset(); } ob_camera_node_ = std::make_unique(this, device_, parameters_); + ob_camera_node_->startStreams(); device_connected_ = true; device_info_ = device_->getDeviceInfo(); CHECK_NOTNULL(device_info_.get()); @@ -340,4 +347,4 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr &list } } // namespace orbbec_camera -RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::OBCameraNodeDriver) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::OBCameraNodeDriver)