mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-11 15:09:48 +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(
|
rclcpp_components_register_node(
|
||||||
${PROJECT_NAME} PLUGIN "orbbec_camera::OBCameraNodeDriver" EXECUTABLE orbbec_camera_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 nodes using the macro
|
||||||
add_orbbec_executable(list_devices_node tools/list_devices_node.cpp)
|
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)
|
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 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
|
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/Device.hpp"
|
||||||
#include "libobsensor/hpp/RecordPlayback.hpp"
|
#include "libobsensor/hpp/RecordPlayback.hpp"
|
||||||
#include "orbbec_camera_msgs/srv/set_bag_recording.hpp"
|
#include "orbbec_camera_msgs/srv/set_bag_recording.hpp"
|
||||||
#include "orbbec_camera_msgs/srv/send_action_command.hpp"
|
|
||||||
|
|
||||||
namespace orbbec_camera {
|
namespace orbbec_camera {
|
||||||
|
|
||||||
@@ -88,10 +87,6 @@ class OBCameraNodeDriver : public rclcpp::Node {
|
|||||||
void setBagRecordingCallback(
|
void setBagRecordingCallback(
|
||||||
const std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Request> request,
|
const std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Request> request,
|
||||||
std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Response> response);
|
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,
|
void presetUpdateCallback(bool firstCall, OBFwUpdateState state, const char* message,
|
||||||
uint8_t percent);
|
uint8_t percent);
|
||||||
void updatePresetFirmware(std::string path);
|
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<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
|
||||||
rclcpp::Service<orbbec_camera_msgs::srv::SetBagRecording>::SharedPtr set_bag_recording_srv_ =
|
rclcpp::Service<orbbec_camera_msgs::srv::SetBagRecording>::SharedPtr set_bag_recording_srv_ =
|
||||||
nullptr;
|
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::chrono::time_point<std::chrono::system_clock> start_time_;
|
||||||
std::string extension_path_;
|
std::string extension_path_;
|
||||||
static backward::SignalHandling sh; // for stack trace
|
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) {
|
if (uid == device_unique_id_ || serial_number_ == serial_number) {
|
||||||
RCLCPP_INFO_STREAM(logger_,
|
RCLCPP_INFO_STREAM(logger_,
|
||||||
"device with " << uid << " disconnected, notify reset device thread");
|
"device with " << uid << " disconnected, notify reset device thread");
|
||||||
send_action_command_srv_.reset();
|
|
||||||
delay_stream_start_after_reconnect_ = true;
|
delay_stream_start_after_reconnect_ = true;
|
||||||
reset_device_flag_ = true;
|
reset_device_flag_ = true;
|
||||||
reset_device_cond_.notify_all();
|
reset_device_cond_.notify_all();
|
||||||
@@ -641,7 +640,6 @@ void OBCameraNodeDriver::resetDevice() {
|
|||||||
// Mark device as disconnected immediately to prevent other threads from accessing it
|
// Mark device as disconnected immediately to prevent other threads from accessing it
|
||||||
device_connected_ = false;
|
device_connected_ = false;
|
||||||
device_connecting_ = false; // Clear connecting flag
|
device_connecting_ = false; // Clear connecting flag
|
||||||
send_action_command_srv_.reset();
|
|
||||||
|
|
||||||
// Stop recording before tearing down the pipeline so the bag file is finalized
|
// Stop recording before tearing down the pipeline so the bag file is finalized
|
||||||
if (record_device_) {
|
if (record_device_) {
|
||||||
@@ -974,105 +972,6 @@ void OBCameraNodeDriver::rebootDeviceCallback(
|
|||||||
return;
|
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(
|
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
|
||||||
const std::shared_ptr<ob::DeviceList> &list) {
|
const std::shared_ptr<ob::DeviceList> &list) {
|
||||||
std::shared_ptr<ob::Device> device = nullptr;
|
std::shared_ptr<ob::Device> device = nullptr;
|
||||||
@@ -1278,7 +1177,6 @@ void OBCameraNodeDriver::initializeBagPlayback() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
|
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
|
||||||
send_action_command_srv_.reset();
|
|
||||||
device_ = device;
|
device_ = device;
|
||||||
updatePresetFirmware(preset_firmware_path_);
|
updatePresetFirmware(preset_firmware_path_);
|
||||||
CHECK_NOTNULL(device_);
|
CHECK_NOTNULL(device_);
|
||||||
@@ -1333,7 +1231,6 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
|
|||||||
serial_number_ = device_info_->getSerialNumber();
|
serial_number_ = device_info_->getSerialNumber();
|
||||||
CHECK_NOTNULL(device_info_.get());
|
CHECK_NOTNULL(device_info_.get());
|
||||||
device_unique_id_ = device_info_->getUid();
|
device_unique_id_ = device_info_->getUid();
|
||||||
setupActionCommandService();
|
|
||||||
|
|
||||||
if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid()) && device_type_ == "camera" &&
|
if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid()) && device_type_ == "camera" &&
|
||||||
!playback_device_) {
|
!playback_device_) {
|
||||||
|
|||||||
Reference in New Issue
Block a user