i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-13 03:30:18 +08:00
Code Issues Packages Projects Releases Wiki Activity

add femto mega ip addr and port

Browse Source
This commit is contained in:
Joe Dong
2024-03-04 14:56:54 +08:00
parent 81b60a3abb
commit 5a0cc23a5f
3 changed files with 33 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
+6
View File
@@ -50,6 +50,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
void startDevice(const std::shared_ptr<ob::DeviceList>& list);
void connectNetDevice(const std::string& net_device_ip, int net_device_port);
void onDeviceConnected(const std::shared_ptr<ob::DeviceList>& device_list);
void onDeviceDisconnected(const std::shared_ptr<ob::DeviceList>& device_list);
@@ -76,6 +78,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
std::string serial_number_;
std::string device_unique_id_;
std::string usb_port_;
bool enumerate_net_device_ = false; // default false
std::shared_ptr<Parameters> parameters_ = nullptr;
std::shared_ptr<std::thread> query_thread_ = nullptr;
std::shared_ptr<std::thread> device_count_update_thread_ = nullptr;
@@ -92,5 +95,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
pthread_mutexattr_t orb_device_lock_attr_;
uint8_t* orb_device_lock_shm_addr_ = nullptr;
int orb_device_lock_shm_fd_ = -1;
// net config
std::string net_device_ip_;
int net_device_port_ = 0;
};
} // namespace orbbec_camera
orbbec_camera/launch/femto_mega.launch.py
+2
View File
@@ -63,6 +63,8 @@ def generate_launch_description():
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('enumerate_net_device', default_value='false'),
DeclareLaunchArgument('net_device_ip', default_value=''),
DeclareLaunchArgument('net_device_port', default_value='0'),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/src/ob_camera_node_driver.cpp
+25 -6
View File
@@ -88,7 +88,9 @@ void OBCameraNodeDriver::init() {
serial_number_ = declare_parameter<std::string>("serial_number", "");
device_num_ = static_cast<int>(declare_parameter<int>("device_num", 1));
usb_port_ = declare_parameter<std::string>("usb_port", "");
auto enumerate_net_device_ = declare_parameter<bool>("enumerate_net_device", "false");
net_device_ip_ = declare_parameter<std::string>("net_device_ip", "");
net_device_port_ = static_cast<int>(declare_parameter<int>("net_device_port", 0));
enumerate_net_device_ = declare_parameter<bool>("enumerate_net_device", false);
ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
ctx_->setDeviceChangedCallback([this](const std::shared_ptr<ob::DeviceList> &removed_list,
const std::shared_ptr<ob::DeviceList> &added_list) {
@@ -163,12 +165,16 @@ void OBCameraNodeDriver::checkConnectTimer() {
void OBCameraNodeDriver::queryDevice() {
if (!device_connected_.load()) {
auto device_list = ctx_->queryDeviceList();
if (device_list->deviceCount() == 0) {
RCLCPP_INFO_STREAM(logger_, "queryDevice : No device found");
return;
if (!net_device_ip_.empty() && net_device_port_ != 0) {
connectNetDevice(net_device_ip_, net_device_port_);
} else {
auto device_list = ctx_->queryDeviceList();
if (device_list->deviceCount() == 0) {
RCLCPP_INFO_STREAM(logger_, "queryDevice : No device found");
return;
}
startDevice(device_list);
}
startDevice(device_list);
}
}
@@ -313,6 +319,19 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
RCLCPP_INFO_STREAM(logger_, "device unique id: " << device_unique_id_);
}
void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int net_device_port) {
if (net_device_ip.empty() || net_device_port == 0) {
RCLCPP_ERROR_STREAM(logger_, "Invalid net device ip or port");
return;
}
auto device = ctx_->createNetDevice(net_device_ip.c_str(), net_device_port);
if (device == nullptr) {
RCLCPP_ERROR_STREAM(logger_, "Failed to connect to net device " << net_device_ip);
return;
}
initializeDevice(device);
}
void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list) {
if (device_connected_) {
return;