mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-04 12:07:46 +08:00
refactor: move Action Command sender to standalone node
This commit is contained in:
@@ -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)
|
||||
@@ -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_) {
|
||||
|
||||
Reference in New Issue
Block a user