diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e3ec4c7..c17b8bb4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs polled_camera + camera_calibration_parsers ) #Get architecture diff --git a/include/avt_vimba_camera/avt_vimba_camera.h b/include/avt_vimba_camera/avt_vimba_camera.h index 75342e4f..f8752ded 100644 --- a/include/avt_vimba_camera/avt_vimba_camera.h +++ b/include/avt_vimba_camera/avt_vimba_camera.h @@ -91,6 +91,10 @@ class AvtVimbaCamera { int getMaxWidth(); int getMaxHeight(); + std::string getCameraName(); + + VmbInt64_t getLutMemorySize(); + // Pass callback function pointer typedef boost::function frameCallbackFunc; void setCallback(frameCallbackFunc callback = &avt_vimba_camera::AvtVimbaCamera::defaultFrameCallback) { @@ -103,6 +107,13 @@ class AvtVimbaCamera { void stopImaging(void); bool isOpened(void) { return opened_; } + // Save camera_info to camera memory + VmbErrorType saveCameraMemory(UcharVector data); + // Load camera_info from camera_memory + VmbErrorType loadCameraMemory(UcharVector& data); + // Convert between Big Endian and Little Endian + UcharVector convertBigEndian(const UcharVector& data); + private: Config config_; @@ -127,6 +138,8 @@ class AvtVimbaCamera { bool show_debug_prints_; std::string name_; + std::string camera_name_; + diagnostic_updater::Updater updater_; CameraState camera_state_; std::string diagnostic_msg_; diff --git a/include/avt_vimba_camera/mono_camera.h b/include/avt_vimba_camera/mono_camera.h index 0b6e6b70..31c51b07 100644 --- a/include/avt_vimba_camera/mono_camera.h +++ b/include/avt_vimba_camera/mono_camera.h @@ -37,6 +37,8 @@ #include #include +#include + #include #include #include @@ -62,6 +64,8 @@ class MonoCamera { ros::NodeHandle nh_; ros::NodeHandle nhp_; + bool memory_loaded_; + std::string ip_; std::string guid_; std::string camera_info_url_; @@ -70,6 +74,8 @@ class MonoCamera { image_transport::ImageTransport it_; // ROS Camera publisher image_transport::CameraPublisher pub_; + // Set camera_info service server + ros::ServiceServer set_camera_info_srv_; @@ -81,6 +87,8 @@ class MonoCamera { typedef dynamic_reconfigure::Server ReconfigureServer; ReconfigureServer reconfigure_server_; + bool setCameraInfo(sensor_msgs::SetCameraInfo::Request& req, sensor_msgs::SetCameraInfo::Response& rsp); + // Camera configuration Config camera_config_; diff --git a/package.xml b/package.xml index 0a2cbe31..c53f9827 100644 --- a/package.xml +++ b/package.xml @@ -24,6 +24,7 @@ sensor_msgs std_msgs polled_camera + camera_calibration_parsers camera_info_manager diagnostic_updater @@ -35,6 +36,7 @@ sensor_msgs std_msgs polled_camera + camera_calibration_parsers diff --git a/src/avt_vimba_camera.cpp b/src/avt_vimba_camera.cpp index 077e3f6d..ae92b6cc 100644 --- a/src/avt_vimba_camera.cpp +++ b/src/avt_vimba_camera.cpp @@ -1127,4 +1127,97 @@ void AvtVimbaCamera::getCurrentState(diagnostic_updater::DiagnosticStatusWrapper break; } } + +VmbErrorType AvtVimbaCamera::saveCameraMemory(UcharVector data) { + // Get memory address + VmbInt64_t memory_address; + getFeatureValue("LUTAddress", memory_address); + + // Uchar is 1 byte long - resize it to 2 bytes + UcharVector data_to_write; + for(UcharVector::iterator it = data.begin() ; it != data.end() ; it++) { + data_to_write.push_back(*it); + data_to_write.push_back(' '); + } + + VmbUint32_t completed_writes = 0; + // Convert Little Endian to Big Endian + UcharVector converted_data(convertBigEndian(data_to_write)); + + // Write to camera memory + VmbErrorType status = vimba_camera_ptr_->WriteMemory(memory_address, converted_data, completed_writes); + + FeaturePtr feature; + status = vimba_camera_ptr_->GetFeatureByName("LUTSave", feature); + if(status != VmbErrorSuccess) { + if(status == VmbErrorNotFound) { + status = vimba_camera_ptr_->GetFeatureByName("LUTSaveAll", feature); + } + } + + status = feature->RunCommand(); + if(status != VmbErrorSuccess) + ROS_ERROR("Save LUT command failed."); + + return status; +} + +VmbErrorType AvtVimbaCamera::loadCameraMemory(UcharVector& data) { + // Get memory address + VmbInt64_t memory_address = 0; + getFeatureValue("LUTAddress", memory_address); + + // Get memory size in bytes + VmbInt64_t lut_size = 0; + getFeatureValue("LUTSizeBytes", lut_size); + + UcharVector tmp_data; + tmp_data.resize(static_cast(lut_size)); + + VmbUint32_t completed_reads = 0; + VmbErrorType status; + + FeaturePtr feature; + status = vimba_camera_ptr_->GetFeatureByName("LUTLoad", feature); + if(status != VmbErrorSuccess) { + if(status == VmbErrorNotFound) { + status = vimba_camera_ptr_->GetFeatureByName("LUTLoadAll", feature); + } + } + + // Read camera memory + status = vimba_camera_ptr_->ReadMemory(memory_address, tmp_data, completed_reads); + + // Convert Big Endian to Little Endian + tmp_data = convertBigEndian(tmp_data); + + // Get only 1 byte from 2 bytes + for(UcharVector::iterator it = tmp_data.begin() ; it != tmp_data.end() ; it += 2) + data.push_back(*it); + + return status; +} + +UcharVector AvtVimbaCamera::convertBigEndian(const UcharVector& data) { + UcharVector tmp(data); + for(size_t i = 0 ; i < tmp.size() ; i += 2) + { + VmbUint16_t *p = reinterpret_cast(&tmp[i]); + *p = ((*p >> 8) & 0xff) | ((*p << 8) & 0xff00); + } + return tmp; +} + +std::string AvtVimbaCamera::getCameraName() +{ + vimba_camera_ptr_->GetModel(camera_name_); + return camera_name_; +} + +VmbInt64_t AvtVimbaCamera::getLutMemorySize() +{ + VmbInt64_t lut_size = 0; + getFeatureValue("LUTSizeBytes", lut_size); + return lut_size; +} } diff --git a/src/mono_camera.cpp b/src/mono_camera.cpp index bf080719..75b4228a 100644 --- a/src/mono_camera.cpp +++ b/src/mono_camera.cpp @@ -46,6 +46,8 @@ MonoCamera::MonoCamera(ros::NodeHandle nh, ros::NodeHandle nhp) : nh_(nh), nhp_( // Set the image publisher before the streaming pub_ = it_.advertiseCamera("image_raw", 1); + memory_loaded_ = false; + // Set the frame callback cam_.setCallback(boost::bind(&avt_vimba_camera::MonoCamera::frameCallback, this, _1)); @@ -58,7 +60,12 @@ MonoCamera::MonoCamera(ros::NodeHandle nh, ros::NodeHandle nhp) : nh_(nh), nhp_( nhp_.param("show_debug_prints", show_debug_prints_, false); // Set camera info manager - info_man_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(nhp_, frame_id, camera_info_url_)); + info_man_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(nh_, frame_id, camera_info_url_)); + + // Service call for setting calibration. + set_camera_info_srv_ = nhp_.advertiseService("set_camera_info", + &MonoCamera::setCameraInfo, + this); // Start dynamic_reconfigure & run configure() reconfigure_server_.setCallback(boost::bind(&avt_vimba_camera::MonoCamera::configure, this, _1, _2)); @@ -132,6 +139,32 @@ void MonoCamera::updateCameraInfo(const avt_vimba_camera::AvtVimbaCameraConfig& ci.roi.height = config.roi_height; ci.roi.width = config.roi_width; + // Load camera_info from camera memory + if(!info_man_->isCalibrated() && !memory_loaded_) { + ROS_WARN("Failed to load camera_info from file. Trying to load camera_info from camera memory ..."); + + UcharVector loaded_data; + VmbErrorType result = cam_.loadCameraMemory(loaded_data); + if(result == VmbErrorSuccess) { + std::string calibration_data; + for(int i = 0 ; i < loaded_data.size() ; i++) + calibration_data += loaded_data[i]; + + std::string cam_name = cam_.getCameraName(); + + std::stringstream yml_stream; + yml_stream << calibration_data; + if(camera_calibration_parsers::readCalibrationYml(yml_stream, cam_name, ci)) { + ROS_INFO("Loaded camera_info from camera memory for camera '%s'.", cam_name.c_str()); + memory_loaded_ = true; + } + else + ROS_WARN("Failed to parse camera_info from camera memory."); + } + else + ROS_WARN("Failed to load camera_info from camera memory."); + } + // set the new URL and load CameraInfo (if any) from it std::string camera_info_url; nhp_.getParam("camera_info_url", camera_info_url); @@ -156,4 +189,64 @@ void MonoCamera::updateCameraInfo(const avt_vimba_camera::AvtVimbaCameraConfig& info_man_->setCameraInfo(ci); } +bool MonoCamera::setCameraInfo(sensor_msgs::SetCameraInfo::Request& req, sensor_msgs::SetCameraInfo::Response& rsp) { + ROS_INFO("New camera_info received."); + + sensor_msgs::CameraInfo &info = req.camera_info; + + if(info.width != info_man_->getCameraInfo().width || info.height != info_man_->getCameraInfo().height) + { + rsp.success = false; + rsp.status_message = (boost::format("camera_info resolution %ix%i does not match current video setting, camera is running at resolution %ix%i.") + % info.width % info.height % info_man_->getCameraInfo().width % info_man_->getCameraInfo().height).str(); + ROS_ERROR("%s", rsp.status_message.c_str()); + return true; + } + + std::string cam_name = cam_.getCameraName(); + + std::stringstream yml_stream; + if(!camera_calibration_parsers::writeCalibrationYml(yml_stream, cam_name, info)) { + rsp.status_message = "Error formatting camera_info for storage."; + rsp.success = false; + } + else { + std::string yml = yml_stream.str(); + + VmbInt64_t lut_size = cam_.getLutMemorySize(); + + if(yml.size() > lut_size / 2) { + rsp.success = false; + rsp.status_message = "Unable to write camera_info to camera memory, exceeded storage capacity."; + } + else { + VmbErrorType status; + UcharVector data; + data.resize(static_cast(lut_size / 2), ' '); + + int index = 0; + for(std::string::iterator it = yml.begin() ; it != yml.end() ; it++) { + data[index] = *it; + index++; + } + + status = cam_.saveCameraMemory(data); + if(status != VmbErrorSuccess) { + rsp.success = false; + rsp.status_message = "Undefinded write to memory problem."; + } + else { + rsp.success = true; + info_man_->setCameraInfo(info); + } + } + } + + if(!rsp.success) + ROS_ERROR("%s", rsp.status_message.c_str()); + + return true; +} + + };