refactor: move Action Command sender to standalone node

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
+18 -1
View File
@@ -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
)
@@ -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 <memory>
#include <rclcpp/rclcpp.hpp>
#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<orbbec_camera_msgs::srv::SendActionCommand::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Response> response);
std::unique_ptr<ob::Context> context_;
rclcpp::Service<orbbec_camera_msgs::srv::SendActionCommand>::SharedPtr
send_action_command_service_;
};
} // namespace orbbec_camera
@@ -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<orbbec_camera_msgs::srv::SetBagRecording::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Response> response);
void setupActionCommandService();
void sendActionCommandCallback(
const std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Response> 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<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
rclcpp::Service<orbbec_camera_msgs::srv::SetBagRecording>::SharedPtr set_bag_recording_srv_ =
nullptr;
rclcpp::Service<orbbec_camera_msgs::srv::SendActionCommand>::SharedPtr send_action_command_srv_ =
nullptr;
std::chrono::time_point<std::chrono::system_clock> start_time_;
std::string extension_path_;
static backward::SignalHandling sh; // for stack trace
@@ -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)
-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_) {