From 62c12ff5427ca1782f635c638aa11459cc002ae6 Mon Sep 17 00:00:00 2001 From: slz Date: Thu, 3 Sep 2026 16:19:45 +0800 Subject: [PATCH] fix(camera): gate Action Command service by device support --- .../orbbec_camera/ob_camera_node_driver.h | 1 + orbbec_camera/src/ob_camera_node_driver.cpp | 70 +++++++++++++++++-- orbbec_camera_msgs/srv/SendActionCommand.srv | 4 +- 3 files changed, 70 insertions(+), 5 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h index 3949f5be..269030cd 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h @@ -88,6 +88,7 @@ class OBCameraNodeDriver : public rclcpp::Node { void setBagRecordingCallback( const std::shared_ptr request, std::shared_ptr response); + void setupActionCommandService(); void sendActionCommandCallback( const std::shared_ptr request, std::shared_ptr response); diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index e146a3e9..83862ce1 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -386,9 +386,6 @@ void OBCameraNodeDriver::init() { set_bag_recording_srv_ = this->create_service( "set_bag_recording", std::bind(&OBCameraNodeDriver::setBagRecordingCallback, this, std::placeholders::_1, std::placeholders::_2)); - send_action_command_srv_ = this->create_service( - "send_action_command", std::bind(&OBCameraNodeDriver::sendActionCommandCallback, this, - std::placeholders::_1, std::placeholders::_2)); 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_; @@ -519,6 +516,7 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptrgetConnectionType(); + if (connection_type == nullptr || std::string(connection_type) != "Ethernet") { + RCLCPP_DEBUG_STREAM(logger_, "Action Command service is unavailable for non-Ethernet device"); + return; + } + + const auto is_supported = [this](OBPropertyID property_id, OBPermissionType permission) { + return device_->isPropertySupported(property_id, permission); + }; + const auto is_readable = [&is_supported](OBPropertyID property_id) { + return is_supported(property_id, OB_PERMISSION_READ) || + is_supported(property_id, OB_PERMISSION_READ_WRITE); + }; + const auto is_writable = [&is_supported](OBPropertyID property_id) { + return is_supported(property_id, OB_PERMISSION_WRITE) || + is_supported(property_id, OB_PERMISSION_READ_WRITE); + }; + + const bool supports_action_command = + is_readable(OB_PROP_ACTION_SIGNAL_COUNT_INT) && + is_writable(OB_PROP_ACTION_DEVICE_KEY_INT) && is_writable(OB_PROP_ACTION_SELECTOR_INT) && + is_writable(OB_PROP_ACTION_GROUP_KEY_INT) && is_writable(OB_PROP_ACTION_GROUP_MASK_INT); + if (!supports_action_command) { + RCLCPP_DEBUG_STREAM(logger_, "Current Ethernet device does not support Action Command"); + return; + } + + const int action_signal_count = device_->getIntProperty(OB_PROP_ACTION_SIGNAL_COUNT_INT); + if (action_signal_count <= 0) { + RCLCPP_DEBUG_STREAM(logger_, "Current device reports no Action Signal blocks"); + return; + } + + send_action_command_srv_ = this->create_service( + "send_action_command", std::bind(&OBCameraNodeDriver::sendActionCommandCallback, this, + std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO_STREAM(logger_, "Action Command service enabled with " + << action_signal_count << " Action Signal block(s)"); + } catch (const ob::Error &e) { + RCLCPP_DEBUG_STREAM(logger_, "Action Command service is unavailable: " + << orbbec_camera::formatObErrorWithStatus(e)); + } catch (const std::exception &e) { + RCLCPP_DEBUG_STREAM(logger_, "Action Command service is unavailable: " << e.what()); + } catch (...) { + RCLCPP_DEBUG_STREAM(logger_, "Action Command service is unavailable"); + } +} + void OBCameraNodeDriver::sendActionCommandCallback( const std::shared_ptr request, std::shared_ptr response) { @@ -985,6 +1039,11 @@ void OBCameraNodeDriver::sendActionCommandCallback( } std::lock_guard lock(device_lock_); + if (!device_connected_.load() || !device_) { + response->success = false; + response->message = "Action Command device is not connected"; + return; + } if (!ctx_) { response->success = false; response->message = "SDK context is not available"; @@ -1000,7 +1059,8 @@ void OBCameraNodeDriver::sendActionCommandCallback( }; response->success = ob_camera_node_ ? ob_camera_node_->withDeviceLock(send_command) : send_command(); - response->message = response->success ? "OK" : "SDK failed to send Action Command"; + response->message = + response->success ? "Action Command dispatched" : "SDK failed to send Action Command"; } catch (const ob::Error &e) { response->success = false; response->message = orbbec_camera::formatObErrorWithStatus(e); @@ -1218,6 +1278,7 @@ void OBCameraNodeDriver::initializeBagPlayback() { } void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &device) { + send_action_command_srv_.reset(); device_ = device; updatePresetFirmware(preset_firmware_path_); CHECK_NOTNULL(device_); @@ -1272,6 +1333,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &dev serial_number_ = device_info_->getSerialNumber(); CHECK_NOTNULL(device_info_.get()); device_unique_id_ = device_info_->getUid(); + setupActionCommandService(); if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid()) && device_type_ == "camera" && !playback_device_) { diff --git a/orbbec_camera_msgs/srv/SendActionCommand.srv b/orbbec_camera_msgs/srv/SendActionCommand.srv index 90c9075e..c8a9628a 100644 --- a/orbbec_camera_msgs/srv/SendActionCommand.srv +++ b/orbbec_camera_msgs/srv/SendActionCommand.srv @@ -3,8 +3,10 @@ uint32 group_key uint32 group_mask # IPv4 destination. Leave empty to use the SDK broadcast address (255.255.255.255). string destination_ip -# PTP timestamp. Zero sends the command immediately. +# GVCP/PTP timestamp: upper 32 bits are seconds and lower 32 bits are nanoseconds. +# Zero sends the command immediately. uint64 scheduled_time --- +# Success means that the SDK dispatched the command; Action Command has no device acknowledgment. bool success string message