change sem to pthread lock

Author: jian-dong

orbbec_camera/CMakeLists.txt

@@ -204,7 +204,6 @@ rclcpp_components_register_node(${PROJECT_NAME}
 )
 # Add nodes using the macro
 add_orbbec_executable(list_devices_node src/list_devices_node.cpp)
-add_orbbec_executable(ob_cleanup_shm_node src/ob_cleanup_shm.cpp)
 add_orbbec_executable(list_depth_work_mode_node src/list_depth_work_mode.cpp)
 add_orbbec_executable(list_camera_profile_mode_node src/list_camera_profile.cpp)
 
@@ -221,7 +220,6 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/)
 install(DIRECTORY ${ORBBEC_INCLUDE_DIR} DESTINATION include)
 install(DIRECTORY ${ORBBEC_LIBS_DIR}/ DESTINATION lib/ FILES_MATCHING PATTERN "*.so*")
 install(TARGETS list_devices_node
-  ob_cleanup_shm_node
   list_depth_work_mode_node
   list_camera_profile_mode_node
   DESTINATION lib/${PROJECT_NAME}/)

orbbec_camera/include/orbbec_camera/constants.h

@@ -110,7 +110,6 @@ const int32_t OPENNI_END_PID = 0x06FF;
 const int32_t ASTRA_MINI_PID = 0x0404;
 const int32_t ASTRA_MINI_S_PID = 0x0407;
 const int GEMINI2_PID = 0x0670;
-const std::string DEFAULT_SEM_NAME = "orbbec_device_sem";
-const key_t DEFAULT_SEM_KEY = 0x0401;
+const std::string ORB_DEFAULT_LOCK_NAME = "orbbec_device_lock";
 
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.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
 #include <atomic>
 #include <thread>
@@ -24,17 +24,10 @@
 #include "dynamic_params.h"
 
 #include "libobsensor/ObSensor.hpp"
+#include <pthread.h>
 
 namespace orbbec_camera {
 
-enum DeviceConnectionEvent {
-  kDeviceConnected = 0,
-  kDeviceDisconnected,
-  kOtherDeviceConnected,
-  kOtherDeviceDisconnected,
-  kDeviceCountUpdate,
-};
-
 class OBCameraNodeDriver : public rclcpp::Node {
  public:
   explicit OBCameraNodeDriver(const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions());
@@ -45,11 +38,6 @@ class OBCameraNodeDriver : public rclcpp::Node {
  private:
   void init();
 
-  void releaseDeviceSemaphore(sem_t* device_sem, int& num_devices_connected);
-
-  void updateConnectedDeviceCount(int& num_devices_connected,
-                                  DeviceConnectionEvent connection_event);
-
   std::shared_ptr<ob::Device> selectDevice(const std::shared_ptr<ob::DeviceList>& list);
 
   std::shared_ptr<ob::Device> selectDeviceBySerialNumber(
@@ -72,8 +60,6 @@ class OBCameraNodeDriver : public rclcpp::Node {
 
   void queryDevice();
 
-  void deviceCountUpdate();
-
   void syncTime();
 
   void resetDevice();
@@ -95,12 +81,16 @@ class OBCameraNodeDriver : public rclcpp::Node {
   std::shared_ptr<std::thread> device_count_update_thread_ = nullptr;
   std::recursive_mutex device_lock_;
   int device_num_ = 1;
-  int num_devices_connected_ = 0;
   rclcpp::TimerBase::SharedPtr check_connect_timer_ = nullptr;
   std::shared_ptr<std::thread> sync_time_thread_ = nullptr;
   std::shared_ptr<std::thread> reset_device_thread_ = nullptr;
   std::mutex reset_device_mutex_;
   std::condition_variable reset_device_cond_;
   std::atomic_bool reset_device_flag_{false};
+  pthread_mutex_t* orb_device_lock_ = nullptr;
+  sem_t* orb_device_sem_ = nullptr;
+  pthread_mutexattr_t orb_device_lock_attr_;
+  uint8_t* orb_device_lock_shm_addr_ = nullptr;
+  int orb_device_lock_shm_fd_ = -1;
 };
 }  // namespace orbbec_camera

orbbec_camera/launch/dabai_dcw.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
         DeclareLaunchArgument('connection_delay', default_value='100'),
         DeclareLaunchArgument('color_width', default_value='640'),
         DeclareLaunchArgument('color_height', default_value='360'),
-        DeclareLaunchArgument('color_fps', default_value='30'),
+        DeclareLaunchArgument('color_fps', default_value='10'),
         DeclareLaunchArgument('color_format', default_value='MJPG'),
         DeclareLaunchArgument('enable_color', default_value='true'),
         DeclareLaunchArgument('flip_color', default_value='false'),
@@ -32,17 +32,17 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
         DeclareLaunchArgument('depth_width', default_value='640'),
         DeclareLaunchArgument('depth_height', default_value='360'),
-        DeclareLaunchArgument('depth_fps', default_value='30'),
+        DeclareLaunchArgument('depth_fps', default_value='10'),
         DeclareLaunchArgument('depth_format', default_value='Y11'),
         DeclareLaunchArgument('enable_depth', default_value='true'),
         DeclareLaunchArgument('flip_depth', default_value='false'),
         DeclareLaunchArgument('depth_qos', default_value='default'),
         DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
         DeclareLaunchArgument('ir_width', default_value='640'),
         DeclareLaunchArgument('ir_height', default_value='480'),
-        DeclareLaunchArgument('ir_fps', default_value='30'),
+        DeclareLaunchArgument('ir_fps', default_value='10'),
         DeclareLaunchArgument('ir_format', default_value='Y10'),
-        DeclareLaunchArgument('enable_ir', default_value='true'),
+        DeclareLaunchArgument('enable_ir', default_value='false'),
         DeclareLaunchArgument('flip_ir', default_value='false'),
         DeclareLaunchArgument('ir_qos', default_value='default'),
         DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),

orbbec_camera/launch/multi_camera.launch.py

@@ -7,35 +7,27 @@
 
 
 def generate_launch_description():
-    # Node configuration
-    cleanup_node = Node(
-        package='orbbec_camera',
-        executable='ob_cleanup_shm_node',
-        name='camera',
-        output='screen'
-    )
-
     # Include launch files
     package_dir = get_package_share_directory('orbbec_camera')
     launch_file_dir = os.path.join(package_dir, 'launch')
     launch1_include = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'dabai_dcw2.launch.py')
+            os.path.join(launch_file_dir, 'dabai_dcw.launch.py')
         ),
         launch_arguments={
             'camera_name': 'camera_01',
-            'usb_port': '5-3.4.4.1.1',
+            'usb_port': '5-3.4.4.3.1',
             'device_num': '2'
         }.items()
     )
 
     launch2_include = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'dabai_dcw2.launch.py')
+            os.path.join(launch_file_dir, 'dabai_dcw.launch.py')
         ),
         launch_arguments={
             'camera_name': 'camera_02',
-            'usb_port': '5-3.4.4.3.1',
+            'usb_port': '5-3.4.4.1.1',
             'device_num': '2'
         }.items()
     )
@@ -44,7 +36,6 @@ def generate_launch_description():
 
     # Launch description
     ld = LaunchDescription([
-        cleanup_node,
         GroupAction([launch1_include]),
         GroupAction([launch2_include]),
     ])

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -21,6 +21,7 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
 #include <csignal>
+#include <sys/mman.h>
 
 namespace orbbec_camera {
 OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
@@ -42,10 +43,6 @@ OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::
 
 OBCameraNodeDriver::~OBCameraNodeDriver() {
   is_alive_.store(false);
-  sem_unlink(DEFAULT_SEM_NAME.c_str());
-  if (int shm_id = shmget(DEFAULT_SEM_KEY, 1, 0666 | IPC_CREAT); shm_id != -1) {
-    shmctl(shm_id, IPC_RMID, nullptr);
-  }
   if (device_count_update_thread_ && device_count_update_thread_->joinable()) {
     device_count_update_thread_->join();
   }
@@ -65,6 +62,27 @@ void OBCameraNodeDriver::init() {
   auto log_level_str = declare_parameter<std::string>("log_level", "none");
   auto log_level = obLogSeverityFromString(log_level_str);
   ob::Context::setLoggerSeverity(log_level);
+  orb_device_lock_shm_fd_ = shm_open(ORB_DEFAULT_LOCK_NAME.c_str(), O_CREAT | O_RDWR, 0666);
+  if (orb_device_lock_shm_fd_ < 0) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to open shared memory " << ORB_DEFAULT_LOCK_NAME);
+    return;
+  }
+  int ret = ftruncate(orb_device_lock_shm_fd_, sizeof(pthread_mutex_t));
+  if (ret < 0) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to truncate shared memory " << ORB_DEFAULT_LOCK_NAME);
+    return;
+  }
+  orb_device_lock_shm_addr_ =
+      static_cast<uint8_t *>(mmap(NULL, sizeof(pthread_mutex_t), PROT_READ | PROT_WRITE, MAP_SHARED,
+                                  orb_device_lock_shm_fd_, 0));
+  if (orb_device_lock_shm_addr_ == MAP_FAILED) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to map shared memory " << ORB_DEFAULT_LOCK_NAME);
+    return;
+  }
+  pthread_mutexattr_init(&orb_device_lock_attr_);
+  pthread_mutexattr_setpshared(&orb_device_lock_attr_, PTHREAD_PROCESS_SHARED);
+  orb_device_lock_ = (pthread_mutex_t *)orb_device_lock_shm_addr_;
+  pthread_mutex_init(orb_device_lock_, &orb_device_lock_attr_);
   is_alive_.store(true);
   parameters_ = std::make_shared<Parameters>(this);
   serial_number_ = declare_parameter<std::string>("serial_number", "");
@@ -79,7 +97,6 @@ void OBCameraNodeDriver::init() {
       this->create_wall_timer(std::chrono::milliseconds(1000), [this]() { checkConnectTimer(); });
   CHECK_NOTNULL(check_connect_timer_);
   query_thread_ = std::make_shared<std::thread>([this]() { queryDevice(); });
-  device_count_update_thread_ = std::make_shared<std::thread>([this]() { deviceCountUpdate(); });
   sync_time_thread_ = std::make_shared<std::thread>([this]() { syncTime(); });
   reset_device_thread_ = std::make_shared<std::thread>([this]() { resetDevice(); });
 }
@@ -89,7 +106,10 @@ void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList>
   if (device_list->deviceCount() == 0) {
     return;
   }
-  RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "onDeviceConnected");
+  pthread_mutex_lock(orb_device_lock_);
+  std::shared_ptr<int> lock_holder(nullptr,
+                                   [this](int *) { pthread_mutex_unlock(orb_device_lock_); });
+ RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "device list count " << device_list->deviceCount());
   if (!device_) {
     try {
       startDevice(device_list);
@@ -109,7 +129,6 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
     return;
   }
   RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected");
-  bool current_device_disconnected = false;
   for (size_t i = 0; i < device_list->deviceCount(); i++) {
     std::string uid = device_list->uid(i);
     std::scoped_lock<decltype(device_lock_)> lock(device_lock_);
@@ -118,14 +137,9 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
       std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
       reset_device_flag_ = true;
       reset_device_cond_.notify_all();
-      current_device_disconnected = true;
       break;
     }
   }
-  auto connect_event = current_device_disconnected
-                           ? DeviceConnectionEvent::kDeviceDisconnected
-                           : DeviceConnectionEvent::kOtherDeviceDisconnected;
-  updateConnectedDeviceCount(num_devices_connected_, connect_event);
 }
 
 OBLogSeverity OBCameraNodeDriver::obLogSeverityFromString(const std::string_view &log_level) {
@@ -157,7 +171,7 @@ void OBCameraNodeDriver::checkConnectTimer() {
 void OBCameraNodeDriver::queryDevice() {
   while (is_alive_ && rclcpp::ok()) {
     if (!device_connected_.load()) {
-      RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "Waiting for device connection...");
+      RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 1000, "Waiting for device connection...");
       auto device_list = ctx_->queryDeviceList();
       if (device_list->deviceCount() == 0) {
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
@@ -170,13 +184,6 @@ void OBCameraNodeDriver::queryDevice() {
   }
 }
 
-void OBCameraNodeDriver::deviceCountUpdate() {
-  while (is_alive_ && rclcpp::ok()) {
-    updateConnectedDeviceCount(num_devices_connected_, DeviceConnectionEvent::kDeviceCountUpdate);
-    std::this_thread::sleep_for(std::chrono::milliseconds(500));
-  }
-}
-
 void OBCameraNodeDriver::syncTime() {
   while (is_alive_ && rclcpp::ok()) {
     if (device_ && device_info_ && !isOpenNIDevice(device_info_->pid())) {
@@ -202,77 +209,14 @@ void OBCameraNodeDriver::resetDevice() {
     reset_device_flag_ = false;
   }
 }
-void OBCameraNodeDriver::releaseDeviceSemaphore(sem_t *device_sem, int &num_devices_connected) {
-  RCLCPP_INFO_THROTTLE(logger_, *get_clock(), 1000, "Release device semaphore");
-  sem_post(device_sem);
-  int sem_value = 0;
-  sem_getvalue(device_sem, &sem_value);
-  RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "semaphore value: " << sem_value);
-  RCLCPP_INFO_THROTTLE(logger_, *get_clock(), 1000, "Release device semaphore done");
-  if (num_devices_connected >= device_num_) {
-    sem_destroy(device_sem);
-    sem_unlink(DEFAULT_SEM_NAME.c_str());
-  }
-}
-
-void OBCameraNodeDriver::updateConnectedDeviceCount(int &num_devices_connected,
-                                                    DeviceConnectionEvent connection_event) {
-  // write connected device count to file
-  int shm_id = shmget(DEFAULT_SEM_KEY, 1, 0666 | IPC_CREAT);
-  if (shm_id == -1) {
-    RCLCPP_INFO_STREAM(logger_, "Failed to create shared memory " << strerror(errno));
-    return;
-  }
-  auto shm_ptr = (int *)shmat(shm_id, nullptr, 0);
-  if (shm_ptr == (void *)-1) {
-    RCLCPP_INFO_STREAM(logger_, "Failed to attach shared memory " << strerror(errno));
-    return;
-  }
-  if (connection_event == DeviceConnectionEvent::kDeviceConnected) {
-    num_devices_connected = *shm_ptr + 1;
-  } else if (connection_event == DeviceConnectionEvent::kDeviceDisconnected && *shm_ptr > 0) {
-    num_devices_connected = *shm_ptr - 1;
-  } else {
-    num_devices_connected = *shm_ptr;
-  }
-  RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
-                               "Current connected device " << num_devices_connected);
-  *shm_ptr = static_cast<int>(num_devices_connected);
-  shmdt(shm_ptr);
-  if (connection_event == DeviceConnectionEvent::kDeviceDisconnected &&
-      num_devices_connected == 0) {
-    shmctl(shm_id, IPC_RMID, nullptr);
-    sem_unlink(DEFAULT_SEM_NAME.c_str());
-  }
-}
 
 std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
     const std::shared_ptr<ob::DeviceList> &list) {
   if (device_num_ == 1) {
     RCLCPP_INFO_STREAM(logger_, "Connecting to the default device");
     return list->getDevice(0);
   }
-  sem_t *device_sem = sem_open(DEFAULT_SEM_NAME.c_str(), O_CREAT, 0644, 1);
-  std::shared_ptr<int> device_sem_guard(nullptr, [&, device_sem](int const *) {
-    if (device_sem != SEM_FAILED) {
-      sem_close(device_sem);
-    }
-  });
-  if (device_sem == SEM_FAILED) {
-    RCLCPP_INFO_STREAM(logger_, "Failed to open semaphore");
-    return nullptr;
-  }
-  RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000,
-                              "Connecting to device with serial number: " << serial_number_);
-  int sem_value = 0;
-  sem_getvalue(device_sem, &sem_value);
-  RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "semaphore value: " << sem_value);
-  int ret = sem_wait(device_sem);
-  if (ret != 0) {
-    RCLCPP_ERROR_STREAM(logger_, "Failed to wait semaphore " << strerror(errno));
-    releaseDeviceSemaphore(device_sem, num_devices_connected_);
-    return nullptr;
-  }
+
   std::shared_ptr<ob::Device> device = nullptr;
   if (!serial_number_.empty()) {
     RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000,
@@ -283,13 +227,6 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
                                 "Connecting to device with usb port: " << usb_port_);
     device = selectDeviceByUSBPort(list, usb_port_);
   }
-  std::shared_ptr<int> sem_guard(nullptr, [&, device](int const *) {
-    auto connect_event = device != nullptr ? DeviceConnectionEvent::kDeviceConnected
-                                           : DeviceConnectionEvent::kOtherDeviceConnected;
-    updateConnectedDeviceCount(num_devices_connected_, connect_event);
-
-    releaseDeviceSemaphore(device_sem, num_devices_connected_);
-  });
   if (device == nullptr) {
     RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Device with serial number %s not found",
                          serial_number_.c_str());