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

Add verification logic in forceIP

Browse Source
This commit is contained in:
obyalian
2025-09-18 14:09:49 +08:00
parent 8a0b92e4f7
commit dffb232816
4 changed files with 72 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/launch/femto_mega.launch.py
+1 -3
View File
@@ -115,10 +115,8 @@ def generate_launch_description():
DeclareLaunchArgument("enable_heartbeat", default_value="false"),
DeclareLaunchArgument("time_domain", default_value="global"),
# If only one camera (network device) is currently connected, you do not need to specify the MAC address.
# Otherwise, you need to specify it yourself. For example, "54:14:FD:06:07:DA"
DeclareLaunchArgument("force_ip_enable", default_value="false"),
DeclareLaunchArgument("force_ip_mac", default_value=""),
DeclareLaunchArgument("force_ip_mac", default_value=""),# you need to specify it yourself. For example, "54:14:FD:06:07:DA"
DeclareLaunchArgument("force_ip_dhcp", default_value="false"),
DeclareLaunchArgument("force_ip_address", default_value="192.168.1.10"),
DeclareLaunchArgument("force_ip_subnet_mask", default_value="255.255.255.0"),
orbbec_camera/launch/gemini435_le.launch.py
+1 -3
View File
@@ -285,10 +285,8 @@ def generate_launch_description():
DeclareLaunchArgument('laser_index0_ir_brightness', default_value='60'),
DeclareLaunchArgument('laser_index0_ir_ae_max_exposure', default_value='30000'),
DeclareLaunchArgument('show_fps_enable', default_value='false'),
# If only one camera (network device) is currently connected, you do not need to specify the MAC address.
# Otherwise, you need to specify it yourself. For example, "54:14:FD:06:07:DA"
DeclareLaunchArgument("force_ip_enable", default_value="false"),
DeclareLaunchArgument("force_ip_mac", default_value=""),
DeclareLaunchArgument("force_ip_mac", default_value=""),# you need to specify it yourself. For example, "54:14:FD:06:07:DA"
DeclareLaunchArgument("force_ip_dhcp", default_value="false"),
DeclareLaunchArgument("force_ip_address", default_value="192.168.1.10"),
DeclareLaunchArgument("force_ip_subnet_mask", default_value="255.255.255.0"),
orbbec_camera/launch/gemini_330_series.launch.py
+2 -3
View File
@@ -297,10 +297,9 @@ def generate_launch_description():
DeclareLaunchArgument('right_ir.image_raw.enable_pub_plugins',default_value='["image_transport/compressed", "image_transport/raw", "image_transport/theora"]'),
DeclareLaunchArgument("force_ip_enable", default_value="false"),
# If only one camera (network device) is currently connected, you do not need to specify the MAC address.
# Otherwise, you need to specify it yourself. For example, "54:14:FD:06:07:DA"
DeclareLaunchArgument("force_ip_mac", default_value=""),
DeclareLaunchArgument("force_ip_dhcp", default_value="false"),
DeclareLaunchArgument("force_ip_dhcp", default_value="false"),# you need to specify it yourself. For example, "54:14:FD:06:07:DA"
DeclareLaunchArgument("force_ip_address", default_value="192.168.1.10"),
DeclareLaunchArgument("force_ip_subnet_mask", default_value="255.255.255.0"),
DeclareLaunchArgument("force_ip_gateway", default_value="192.168.1.1"),
orbbec_camera/src/ob_camera_node_driver.cpp
+68 -18
View File
@@ -1003,9 +1003,11 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
bool OBCameraNodeDriver::applyForceIpConfig() {
if (!force_ip_enable_) {
RCLCPP_DEBUG(logger_, "[ForceIP] Disabled, skip config");
return false;
}
if (force_ip_success_) {
RCLCPP_DEBUG(logger_, "[ForceIP] Already applied, skip");
return false;
}
@@ -1013,21 +1015,68 @@ bool OBCameraNodeDriver::applyForceIpConfig() {
config.dhcp = force_ip_dhcp_ ? 1 : 0;
if (config.dhcp == 0) {
if (force_ip_address_.empty() || force_ip_subnet_mask_.empty() || force_ip_gateway_.empty()) {
RCLCPP_WARN(logger_, "Force IP enabled but parameters are incomplete");
return false;
}
auto parseIp = [](const std::string &ip, uint8_t out[4]) {
std::stringstream ss(ip);
RCLCPP_INFO(logger_, "[ForceIP] Static config mode");
auto strToIp = [&](const std::string &s, uint8_t out[4]) -> bool {
std::stringstream ss(s);
std::string item;
int i = 0;
while (std::getline(ss, item, '.') && i < 4) {
out[i++] = static_cast<uint8_t>(std::stoi(item));
int val = std::stoi(item);
if (val < 0 || val > 255) return false;
out[i++] = static_cast<uint8_t>(val);
}
return i == 4;
};
parseIp(force_ip_address_, config.address);
parseIp(force_ip_subnet_mask_, config.mask);
parseIp(force_ip_gateway_, config.gateway);
auto ipArrayToUint = [&](const uint8_t ip[4]) -> uint32_t {
return (static_cast<uint32_t>(ip[0]) << 24) | (static_cast<uint32_t>(ip[1]) << 16) |
(static_cast<uint32_t>(ip[2]) << 8) | (static_cast<uint32_t>(ip[3]));
};
uint8_t ip[4], mask[4], gw[4];
if (!strToIp(force_ip_address_, ip)) {
RCLCPP_ERROR(logger_, "[ForceIP] Invalid IP: %s", force_ip_address_.c_str());
return false;
}
if (!strToIp(force_ip_subnet_mask_, mask)) {
RCLCPP_ERROR(logger_, "[ForceIP] Invalid Mask: %s", force_ip_subnet_mask_.c_str());
return false;
}
if (!strToIp(force_ip_gateway_, gw)) {
RCLCPP_ERROR(logger_, "[ForceIP] Invalid Gateway: %s", force_ip_gateway_.c_str());
return false;
}
uint32_t ipVal = ipArrayToUint(ip);
uint32_t maskVal = ipArrayToUint(mask);
uint32_t gwVal = ipArrayToUint(gw);
if (ipVal == 0 || ipVal == 0xFFFFFFFF) {
RCLCPP_ERROR(logger_, "[ForceIP] Illegal IP: %s", force_ip_address_.c_str());
return false;
}
if (maskVal == 0 || maskVal == 0xFFFFFFFF) {
RCLCPP_ERROR(logger_, "[ForceIP] Illegal Mask (all 0 or all 1): %s",
force_ip_subnet_mask_.c_str());
return false;
}
uint32_t inverted = ~maskVal + 1;
if ((inverted & (inverted - 1)) != 0) {
RCLCPP_ERROR(logger_, "[ForceIP] Illegal Mask (non-contiguous bits): %s",
force_ip_subnet_mask_.c_str());
return false;
}
if ((ipVal & ~maskVal) == 0 || (ipVal & ~maskVal) == ~maskVal) {
RCLCPP_ERROR(logger_, "[ForceIP] Illegal host IP (network/broadcast addr): %s",
force_ip_address_.c_str());
return false;
}
if ((ipVal & maskVal) != (gwVal & maskVal)) {
RCLCPP_ERROR(logger_, "[ForceIP] Gateway %s not in same subnet as IP %s",
force_ip_gateway_.c_str(), force_ip_address_.c_str());
return false;
}
std::memcpy(config.address, ip, 4);
std::memcpy(config.mask, mask, 4);
std::memcpy(config.gateway, gw, 4);
}
force_ip_success_ = false;
@@ -1040,25 +1089,26 @@ bool OBCameraNodeDriver::applyForceIpConfig() {
} else if (device_list->getCount() == 1) {
mac = device_list->getUid(index);
} else {
RCLCPP_ERROR(logger_, "MAC address is empty");
RCLCPP_ERROR(logger_, "[ForceIP] MAC address is empty");
return false;
}
if (ctx_->changeNetDeviceIpConfig(mac.c_str(), config)) {
RCLCPP_INFO(logger_,
"Force IP config applied. force_ip_dhcp=%d ip=%s mask=%s force_ip_gateway=%s",
config.dhcp, force_ip_address_.c_str(), force_ip_subnet_mask_.c_str(),
RCLCPP_INFO(logger_, "[ForceIP] Config applied. dhcp=%d ip=%s mask=%s gw=%s", config.dhcp,
force_ip_address_.c_str(), force_ip_subnet_mask_.c_str(),
force_ip_gateway_.c_str());
force_ip_success_ = true;
} else {
RCLCPP_ERROR(logger_, "Failed to apply Force IP config (SDK returned false)");
RCLCPP_ERROR(logger_, "[ForceIP] Failed to apply config (SDK returned false)");
}
} catch (const ob::Error &e) {
RCLCPP_ERROR(logger_, "Force IP config failed with ob::Error: %s", e.getMessage());
RCLCPP_ERROR(logger_, "[ForceIP] ob::Error: %s", e.getMessage());
} catch (const std::exception &e) {
RCLCPP_ERROR(logger_, "Force IP config failed with std::exception: %s", e.what());
RCLCPP_ERROR(logger_, "[ForceIP] std::exception: %s", e.what());
} catch (...) {
RCLCPP_ERROR(logger_, "Force IP config failed with unknown error");
RCLCPP_ERROR(logger_, "[ForceIP] Unknown error");
}
return force_ip_success_;
}