i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-07 21:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor: move Action Command sender to standalone node

Browse Source
This commit is contained in:
slz
2026-09-03 17:21:50 +08:00
parent 62c12ff542
commit 0e6b43e2d4
5 changed files with 138 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/gige_action_command_node.cpp
+78
View File
@@ -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 <exception>
#include <functional>
#include <sstream>
#include <string>
#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<int>(error.getStatus());
return stream.str();
}
} // namespace
GigEActionCommandNode::GigEActionCommandNode(const rclcpp::NodeOptions& node_options)
: Node("gige_action_command_node", node_options), context_(std::make_unique<ob::Context>()) {
context_->enableNetDeviceEnumeration(true);
send_action_command_service_ = create_service<orbbec_camera_msgs::srv::SendActionCommand>(
"~/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<orbbec_camera_msgs::srv::SendActionCommand::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Response> 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)
orbbec_camera/src/ob_camera_node_driver.cpp
-103
View File
@@ -516,7 +516,6 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
if (uid == device_unique_id_ || serial_number_ == serial_number) {
RCLCPP_INFO_STREAM(logger_,
"device with " << uid << " disconnected, notify reset device thread");
send_action_command_srv_.reset();
delay_stream_start_after_reconnect_ = true;
reset_device_flag_ = true;
reset_device_cond_.notify_all();
@@ -641,7 +640,6 @@ void OBCameraNodeDriver::resetDevice() {
// Mark device as disconnected immediately to prevent other threads from accessing it
device_connected_ = false;
device_connecting_ = false; // Clear connecting flag
send_action_command_srv_.reset();
// Stop recording before tearing down the pipeline so the bag file is finalized
if (record_device_) {
@@ -974,105 +972,6 @@ void OBCameraNodeDriver::rebootDeviceCallback(
return;
}
void OBCameraNodeDriver::setupActionCommandService() {
send_action_command_srv_.reset();
if (!device_ || !device_info_ || device_type_ != "camera" || playback_device_) {
return;
}
try {
const char *connection_type = device_info_->getConnectionType();
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<orbbec_camera_msgs::srv::SendActionCommand>(
"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<orbbec_camera_msgs::srv::SendActionCommand::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Response> response) {
if (!request) {
response->success = false;
response->message = "Invalid request";
return;
}
std::lock_guard<decltype(device_lock_)> 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<ob::Device> OBCameraNodeDriver::selectDevice(
const std::shared_ptr<ob::DeviceList> &list) {
std::shared_ptr<ob::Device> device = nullptr;
@@ -1278,7 +1177,6 @@ void OBCameraNodeDriver::initializeBagPlayback() {
}
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &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<ob::Device> &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_) {