From 0e6b43e2d40c15e17c2917f63d27ad212fd7d6fa Mon Sep 17 00:00:00 2001 From: slz Date: Thu, 3 Sep 2026 17:21:50 +0800 Subject: [PATCH] refactor: move Action Command sender to standalone node --- orbbec_camera/CMakeLists.txt | 19 +++- .../orbbec_camera/gige_action_command_node.h | 42 +++++++ .../orbbec_camera/ob_camera_node_driver.h | 7 -- .../src/gige_action_command_node.cpp | 78 +++++++++++++ orbbec_camera/src/ob_camera_node_driver.cpp | 103 ------------------ 5 files changed, 138 insertions(+), 111 deletions(-) create mode 100644 orbbec_camera/include/orbbec_camera/gige_action_command_node.h create mode 100644 orbbec_camera/src/gige_action_command_node.cpp diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt index 3d64408e..b5ad08ac 100644 --- a/orbbec_camera/CMakeLists.txt +++ b/orbbec_camera/CMakeLists.txt @@ -269,6 +269,22 @@ orbbec_target_dependencies(${PROJECT_NAME} rclcpp_components_register_node( ${PROJECT_NAME} PLUGIN "orbbec_camera::OBCameraNodeDriver" EXECUTABLE orbbec_camera_node ) + +add_library(gige_action_command_component SHARED src/gige_action_command_node.cpp) +target_include_directories(gige_action_command_component PUBLIC ${COMMON_INCLUDE_DIRS}) +target_link_directories(gige_action_command_component PRIVATE ${ORBBEC_LIBS_DIR}) +target_link_libraries(gige_action_command_component + ${orbbec_camera_msgs_TARGETS} + OrbbecSDK + rclcpp::rclcpp +) +orbbec_target_dependencies(gige_action_command_component rclcpp_components) +rclcpp_components_register_node( + gige_action_command_component + PLUGIN "orbbec_camera::GigEActionCommandNode" + EXECUTABLE gige_action_command_node +) + # Add nodes using the macro add_orbbec_executable(list_devices_node tools/list_devices_node.cpp) add_orbbec_executable(list_depth_work_mode_node tools/list_depth_work_mode.cpp) @@ -360,7 +376,8 @@ rclcpp_components_register_node( ) # Install rules -install(TARGETS ${PROJECT_NAME} frame_latency start_benchmark multi_save_rgbir ARCHIVE DESTINATION lib +install(TARGETS ${PROJECT_NAME} gige_action_command_component frame_latency start_benchmark + multi_save_rgbir ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) diff --git a/orbbec_camera/include/orbbec_camera/gige_action_command_node.h b/orbbec_camera/include/orbbec_camera/gige_action_command_node.h new file mode 100644 index 00000000..0e311238 --- /dev/null +++ b/orbbec_camera/include/orbbec_camera/gige_action_command_node.h @@ -0,0 +1,42 @@ +/******************************************************************************* + * Copyright (c) 2026 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 + +#include + +#include "libobsensor/hpp/Context.hpp" +#include "orbbec_camera_msgs/srv/send_action_command.hpp" + +namespace orbbec_camera { + +class GigEActionCommandNode : public rclcpp::Node { + public: + explicit GigEActionCommandNode(const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()); + + private: + void sendActionCommandCallback( + const std::shared_ptr request, + std::shared_ptr response); + + std::unique_ptr context_; + rclcpp::Service::SharedPtr + send_action_command_service_; +}; + +} // namespace orbbec_camera 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 269030cd..a132d177 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h @@ -33,7 +33,6 @@ #include "libobsensor/hpp/Device.hpp" #include "libobsensor/hpp/RecordPlayback.hpp" #include "orbbec_camera_msgs/srv/set_bag_recording.hpp" -#include "orbbec_camera_msgs/srv/send_action_command.hpp" namespace orbbec_camera { @@ -88,10 +87,6 @@ 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); void presetUpdateCallback(bool firstCall, OBFwUpdateState state, const char* message, uint8_t percent); void updatePresetFirmware(std::string path); @@ -153,8 +148,6 @@ class OBCameraNodeDriver : public rclcpp::Node { rclcpp::Service::SharedPtr reboot_device_srv_ = nullptr; rclcpp::Service::SharedPtr set_bag_recording_srv_ = nullptr; - rclcpp::Service::SharedPtr send_action_command_srv_ = - nullptr; std::chrono::time_point start_time_; std::string extension_path_; static backward::SignalHandling sh; // for stack trace diff --git a/orbbec_camera/src/gige_action_command_node.cpp b/orbbec_camera/src/gige_action_command_node.cpp new file mode 100644 index 00000000..f1e201b5 --- /dev/null +++ b/orbbec_camera/src/gige_action_command_node.cpp @@ -0,0 +1,78 @@ +/******************************************************************************* + * Copyright (c) 2026 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. + *******************************************************************************/ + +#include "orbbec_camera/gige_action_command_node.h" + +#include +#include +#include +#include + +#include "rclcpp_components/register_node_macro.hpp" + +namespace orbbec_camera { +namespace { + +std::string formatObError(const ob::Error& error) { + std::ostringstream stream; + stream << (error.getMessage() ? error.getMessage() : "Unknown OB error") + << " status:" << static_cast(error.getStatus()); + return stream.str(); +} + +} // namespace + +GigEActionCommandNode::GigEActionCommandNode(const rclcpp::NodeOptions& node_options) + : Node("gige_action_command_node", node_options), context_(std::make_unique()) { + context_->enableNetDeviceEnumeration(true); + send_action_command_service_ = create_service( + "~/send_action_command", std::bind(&GigEActionCommandNode::sendActionCommandCallback, this, + std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO(get_logger(), "GigE Action Command service is ready"); +} + +void GigEActionCommandNode::sendActionCommandCallback( + const std::shared_ptr request, + std::shared_ptr response) { + if (!request) { + response->success = false; + response->message = "Invalid request"; + return; + } + + const std::string destination_ip = + request->destination_ip.empty() ? "255.255.255.255" : request->destination_ip; + try { + response->success = + context_->sendActionCommand(request->device_key, request->group_key, request->group_mask, + destination_ip.c_str(), request->scheduled_time); + response->message = + response->success ? "Action Command dispatched" : "SDK failed to send Action Command"; + } catch (const ob::Error& error) { + response->success = false; + response->message = formatObError(error); + } catch (const std::exception& error) { + response->success = false; + response->message = error.what(); + } catch (...) { + response->success = false; + response->message = "Unknown error"; + } +} + +} // namespace orbbec_camera + +RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::GigEActionCommandNode) diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index 83862ce1..a22d6d09 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -516,7 +516,6 @@ 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) { - if (!request) { - response->success = false; - response->message = "Invalid request"; - return; - } - - 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"; - return; - } - - const std::string destination_ip = - request->destination_ip.empty() ? "255.255.255.255" : request->destination_ip; - try { - const auto send_command = [this, &request, &destination_ip]() { - return ctx_->sendActionCommand(request->device_key, request->group_key, request->group_mask, - destination_ip.c_str(), request->scheduled_time); - }; - response->success = - ob_camera_node_ ? ob_camera_node_->withDeviceLock(send_command) : send_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); - } catch (const std::exception &e) { - response->success = false; - response->message = e.what(); - } catch (...) { - response->success = false; - response->message = "unknown error"; - } -} - std::shared_ptr OBCameraNodeDriver::selectDevice( const std::shared_ptr &list) { std::shared_ptr device = nullptr; @@ -1278,7 +1177,6 @@ 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_); @@ -1333,7 +1231,6 @@ 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_) {