Added feature for loading and saving camera_info from/to camera flash memory.

CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   std_msgs
   polled_camera
+  camera_calibration_parsers
 )
 
 #Get architecture

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<void (const FramePtr)> 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_;

include/avt_vimba_camera/mono_camera.h

@@ -37,6 +37,8 @@
 #include <avt_vimba_camera/AvtVimbaCameraConfig.h>
 #include <avt_vimba_camera/avt_vimba_api.h>
 
+#include <camera_calibration_parsers/parse_yml.h>
+
 #include <ros/ros.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/CameraInfo.h>
@@ -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<Config> ReconfigureServer;
   ReconfigureServer reconfigure_server_;
 
+  bool setCameraInfo(sensor_msgs::SetCameraInfo::Request& req, sensor_msgs::SetCameraInfo::Response& rsp);
+
   // Camera configuration
   Config camera_config_;
 

package.xml

@@ -24,6 +24,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>polled_camera</build_depend>
+  <build_depend>camera_calibration_parsers</build_depend>
   <!--build_depend>libvimba</build_depend-->
   <run_depend>camera_info_manager</run_depend>
   <run_depend>diagnostic_updater</run_depend>
@@ -35,6 +36,7 @@
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>polled_camera</run_depend>
+  <run_depend>camera_calibration_parsers</run_depend>
   <!--run_depend>libvimba</run_depend-->
 
 </package>

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<size_t>(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<VmbUint16_t*>(&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;
+}
 }

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<camera_info_manager::CameraInfoManager>(new camera_info_manager::CameraInfoManager(nhp_, frame_id, camera_info_url_));
+  info_man_  = boost::shared_ptr<camera_info_manager::CameraInfoManager>(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<size_t>(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;
+}
+
+
 };