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

Add update Preset Firmware function, associated parameter preset_firmware_path

Browse Source
This commit is contained in:
jj
2025-02-27 15:08:35 +08:00
parent 769a129f84
commit 2320ff1ad3
3 changed files with 113 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
+4
View File
@@ -68,6 +68,9 @@ class OBCameraNodeDriver : public rclcpp::Node {
void rebootDeviceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response);
void presetUpdateCallback(bool firstCall, OBFwUpdateState state, const char* message,
uint8_t percent);
void updatePresetFirmware(std::string path);
private:
const rclcpp::NodeOptions node_options_;
@@ -104,6 +107,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
int net_device_port_ = 0;
int connection_delay_ = 100;
bool enable_sync_host_time_ = true;
std::string preset_firmware_path_;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
std::chrono::time_point<std::chrono::system_clock> start_time_;
std::string extension_path_;
orbbec_camera/launch/gemini_330_series.launch.py
+2
View File
@@ -56,6 +56,7 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('preset_firmware_path', default_value=''),
DeclareLaunchArgument('uvc_backend', default_value='libuvc'),#libuvc or v4l2
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
@@ -255,6 +256,7 @@ def generate_launch_description():
),
],
output="screen",
# prefix=["gdb -ex run --args"]
)
])
]
orbbec_camera/src/ob_camera_node_driver.cpp
+107
View File
@@ -131,6 +131,8 @@ void OBCameraNodeDriver::init() {
enable_sync_host_time_ = declare_parameter<bool>("enable_sync_host_time", true);
g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
g_time_domain = declare_parameter<std::string>("time_domain", g_time_domain);
preset_firmware_path_ =
declare_parameter<std::string>("preset_firmware_path", preset_firmware_path_);
ob::Context::setLoggerToConsole(log_level);
orb_device_lock_shm_fd_ = shm_open(ORB_DEFAULT_LOCK_NAME.c_str(), O_CREAT | O_RDWR, 0666);
if (orb_device_lock_shm_fd_ < 0) {
@@ -392,6 +394,7 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
device_ = device;
updatePresetFirmware(preset_firmware_path_);
CHECK_NOTNULL(device_);
CHECK_NOTNULL(device_.get());
if (ob_camera_node_) {
@@ -555,6 +558,110 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
reset_device_cond_.notify_all();
}
}
void OBCameraNodeDriver::updatePresetFirmware(std::string path) {
if (path.empty()) {
return;
} else {
std::stringstream ss(path);
std::string path_segment;
std::vector<std::string> paths;
OBFwUpdateState updateState = STAT_START;
bool firstCall = true;
while (std::getline(ss, path_segment, ',')) {
paths.push_back(path_segment);
}
uint8_t index = 0;
uint8_t count = static_cast<uint8_t>(paths.size());
char(*filePaths)[OB_PATH_MAX] = new char[count][OB_PATH_MAX];
RCLCPP_INFO_STREAM(this->get_logger(), "paths.cout : " << (uint32_t)count);
for (const auto &p : paths) {
strcpy(filePaths[index], p.c_str());
RCLCPP_INFO_STREAM(this->get_logger(),
"path: " << (uint32_t)index << ":" << filePaths[index]);
index++;
}
RCLCPP_INFO_STREAM(this->get_logger(),
"Start to update optional depth preset, please wait a moment...");
try {
device_->updateOptionalDepthPresets(
filePaths, count,
[this, &updateState, &firstCall](OBFwUpdateState state, const char *message,
uint8_t percent) {
updateState = state;
presetUpdateCallback(firstCall, state, message, percent);
// firstCall = false;
});
delete[] filePaths;
filePaths = nullptr;
if (updateState == STAT_DONE || updateState == STAT_DONE_WITH_DUPLICATES) {
RCLCPP_INFO_STREAM(this->get_logger(), "After updating the preset: ");
auto presetList = device_->getAvailablePresetList();
RCLCPP_INFO_STREAM(this->get_logger(), "Preset count: " << presetList->getCount());
for (uint32_t i = 0; i < presetList->getCount(); ++i) {
RCLCPP_INFO_STREAM(this->get_logger(), " - " << presetList->getName(i));
}
RCLCPP_INFO_STREAM(this->get_logger(),
"Current preset: " << device_->getCurrentPresetName());
std::string key = "PresetVer";
if (device_->isExtensionInfoExist(key)) {
std::string value = device_->getExtensionInfo(key);
RCLCPP_INFO_STREAM(this->get_logger(), "Preset version: " << value);
} else {
RCLCPP_INFO_STREAM(this->get_logger(), "PresetVer: ");
}
}
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware " << e.getMessage());
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware");
}
}
}
void OBCameraNodeDriver::presetUpdateCallback(bool firstCall, OBFwUpdateState state,
const char *message, uint8_t percent) {
if (!firstCall) {
std::cout << "\033[3F";
}
std::cout << "\033[K";
std::cout << "Progress: " << static_cast<uint32_t>(percent) << "%" << std::endl;
std::cout << "\033[K";
std::cout << "Status : ";
switch (state) {
case STAT_VERIFY_SUCCESS:
std::cout << "Image file verification success" << std::endl;
break;
case STAT_FILE_TRANSFER:
std::cout << "File transfer in progress" << std::endl;
break;
case STAT_DONE:
std::cout << "Update completed" << std::endl;
break;
case STAT_DONE_WITH_DUPLICATES:
std::cout << "Update completed, duplicated presets have been ignored" << std::endl;
break;
case STAT_IN_PROGRESS:
std::cout << "Update in progress" << std::endl;
break;
case STAT_START:
std::cout << "Starting the update" << std::endl;
break;
case STAT_VERIFY_IMAGE:
std::cout << "Verifying image file" << std::endl;
break;
default:
std::cout << "Unknown status or error" << std::endl;
break;
}
std::cout << "\033[K";
std::cout << "Message : " << message << std::endl << std::flush;
}
} // namespace orbbec_camera
RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::OBCameraNodeDriver)