{"action":"created","comment":{"url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/comments/173506890","pull_request_review_id":102387174,"id":173506890,"diff_hunk":"@@ -40,1372 +40,899 @@\n  *  https://int80k.com/libuvc/doc/\n  */\n \n+#include <functional>\n+#include <iostream>\n+\n+#include <cv_bridge/cv_bridge.h>\n+#include <sensor_msgs/image_encodings.h>\n+\n #include \"uvc_ros_driver.h\"\n \n-namespace uvc\n-{\n+namespace uvc {\n \n-static void callback(uvc_frame *frame, void *arg)\n-{\n-\tuvcROSDriver *obj = (uvcROSDriver *)arg;\n-\tobj->uvc_cb(frame);\n+static void callback(uvc_frame *frame, void *arg) {\n+  uvcROSDriver *obj = (uvcROSDriver *)arg;\n+  obj->uvc_cb(frame);\n }\n \n-static bool myPairMax(std::pair<int, int> p, std::pair<int, int> p1)\n-{\n-\t// gratest value is supposed be on the second index\n-\treturn p.second < p1.second;\n+static bool myPairMax(std::pair<int, int> p, std::pair<int, int> p1) {\n+  // gratest value is supposed be on the second index\n+  return p.second < p1.second;\n }\n \n-uvcROSDriver::~uvcROSDriver()\n-{\n-\tif(serial_port_open_){\n-\t\tsetParam(\"CAMERA_ENABLE\", 0.0f);\n-\t}\n-\tmavlink_message_t message;\n-\tmavlink_param_value_t param;\n-\tmavlink_heartbeat_t heartbeat;\n-\tint count = 0;\n-\tbool wait=1;\n-\tstd::string str = \"CAMERA_ENABLE\";\n-\n-\tif(serial_port_open_){\n-\t\twhile(wait){\n-\t\t\tint res = sp_.read_message(message);\n-\t\t\tif(res == -1){\n-\t\t\t\tserial_port_open_ = false;\n-\t\t\t\tbreak;\n-\t\t\t}\n-\t\t\tif(res != 0){\n-\t\t\t\tif(message.msgid==22){\n-\t\t\t\t\tmavlink_msg_param_value_decode(&message, &param);\n-\t\t\t\t\tif(str.compare(param.param_id)==0 && param.param_value==0){\n-\t\t\t\t\t\twait = 0;\n-\t\t\t\t\t}\n-\t\t\t\t\telse{\n-\t\t\t\t\t\tsetParam(\"CAMERA_ENABLE\", 0.0f);\n-\t\t\t\t\t}\n-\t\t\t\t\t//std::cout << \"received id \" << param.param_id << \" value \" << param.param_value <<\" iteration \" \t\t\t\t<< (float) count << std::endl;\n-\t\t\t\t\tcount++;\n-\t\t\t\t}\n-\t\t\t}\n-\t\t}\n-\n-\tprintf(\"Camera Disabled \\n\");\n-\t// close serial port\n-\tsp_.close_serial();\n-\tusleep(500000);\n-\tuvc_stop_streaming(devh_);\n-\n-\t// close uvc device\n-\tuvc_close(devh_);\n-\tprintf(\"Device closed\");\n-\t// ROS_INFO(\"Device closed\");\n-\tuvc_unref_device(dev_);\n-\tuvc_exit(ctx_);\n-\t}\n+uvcROSDriver::~uvcROSDriver() {\n+  shutdown_ = true;\n+\n+  if (serial_port_open_) {\n+    setParam(\"CAMERA_ENABLE\", 0.0f);\n+  }\n+  mavlink_message_t message;\n+  mavlink_param_value_t param;\n+  bool wait = 1;\n+  std::string str = \"CAMERA_ENABLE\";\n+\n+  if (serial_port_open_) {\n+    while (wait) {\n+      int res = sp_.read_message(message);\n+      if (res == -1) {\n+        serial_port_open_ = false;\n+        break;\n+      }\n+      if (res != 0) {\n+        if (message.msgid == 22) {\n+          mavlink_msg_param_value_decode(&message, &param);\n+          if (str.compare(param.param_id) == 0 && param.param_value == 0) {\n+            wait = 0;\n+          } else {\n+            setParam(\"CAMERA_ENABLE\", 0.0f);\n+          }\n+        }\n+      }\n+    }\n+\n+    printf(\"Camera Disabled \\n\");\n+    // close serial port\n+    sp_.close_serial();\n+    usleep(500000);\n+    uvc_stop_streaming(devh_);\n+\n+    // close uvc device\n+    uvc_close(devh_);\n+    printf(\"Device closed\");\n+    uvc_unref_device(dev_);\n+    uvc_exit(ctx_);\n+  }\n }\n \n ////////////////////////////////////////////////////////////////////////////////\n \n-void uvcROSDriver::initDevice()\n-{\n-\t// initialize serial port\n-\t// sp_ = Serial_Port(\"/dev/ttyUSB0\", 115200);\n-\tsp_ = Serial_Port(\"/dev/serial/by-id/usb-Cypress_FX3-if02\", 115200);\n-\n-        bool first_fault = true;\n-\tint open = 0;\n-        while (true)\n-        {\n-        \tros::spinOnce();\n-\t\tif(!nh_.ok())\n-\t\t\treturn;\n-        \topen = sp_.open_serial();\n-\n-        \tif (open != -1){\n-\t\t\tserial_port_open_ = true;\n-          \t\tbreak;\n-\t\t}\n-       \t\tif (first_fault)\n-        \t{\n-          \t\tROS_ERROR(\"Couldn't open serialport /dev/serial/by-id/usb-Cypress_FX3-if02. Will retry every second.\");\n-          \t\tfirst_fault = false;\n-       \t\t }\n-        \tsleep(1.0);\n+void uvcROSDriver::initDevice() {\n+  // initialize serial port\n+  // sp_ = Serial_Port(\"/dev/ttyUSB0\", 115200);\n+  sp_ = Serial_Port(\"/dev/serial/by-id/usb-Cypress_FX3-if02\", 115200);\n+\n+  bool first_fault = true;\n+  int open = 0;\n+  while (true) {\n+    ros::spinOnce();\n+    if (!nh_.ok()) return;\n+    open = sp_.open_serial();\n+\n+    if (open != -1) {\n+      serial_port_open_ = true;\n+      break;\n+    }\n+    if (first_fault) {\n+      ROS_ERROR(\n+          \"Couldn't open serialport /dev/serial/by-id/usb-Cypress_FX3-if02. \"\n+          \"Will retry every second.\");\n+      first_fault = false;\n+    }\n+    sleep(1.0);\n+  }\n+\n+  // initialize camera image publisher\n+  constexpr int kCamQueueSize = 5;\n+  constexpr int kIMUQueueSize = 20;\n+  std::string prev_topic;\n+  for (size_t i = 0; i < n_cameras_; ++i) {\n+    const std::string topic = \"cam_\" + std::to_string(i) + \"/\";\n+    cam_raw_pubs_.emplace_back(\n+        it_.advertise(topic + \"image_raw\", kCamQueueSize));\n+    cam_info_pubs_.emplace_back(\n+        nh_.advertise<sensor_msgs::CameraInfo>(topic + \"camera_info\", 1000));\n+\n+    if (i % 2) {\n+      cam_rect_pubs_.emplace_back(\n+          it_.advertise(prev_topic + \"image_rect\", kCamQueueSize));\n+      cam_disp_pubs_.emplace_back(\n+          it_.advertise(prev_topic + \"image_depth\", kCamQueueSize));\n+\n+      imu_pubs_.emplace_back(\n+          nh_.advertise<sensor_msgs::Imu>(prev_topic + \"imu\", kIMUQueueSize));\n+      imu_pubs_.emplace_back(\n+          nh_.advertise<sensor_msgs::Imu>(topic + \"imu\", kIMUQueueSize));\n+    }\n+    prev_topic = topic;\n+  }\n+\n+  info_cams_.resize(n_cameras_);\n+\n+  // time translator\n+  constexpr int kSecondsToNanoSeconds = 1e9;\n+  device_time_translator_.reset(\n+      new cuckoo_time_translator::UnwrappedDeviceTimeTranslator(\n+          cuckoo_time_translator::ClockParameters(kSecondsToNanoSeconds),\n+          nh_.getNamespace(),\n+          cuckoo_time_translator::Defaults().setFilterAlgorithm(\n+              cuckoo_time_translator::FilterAlgorithm::ConvexHull)));\n+\n+  // wait on heartbeat\n+  std::cout << \"Waiting on device.\";\n+  fflush(stdout);\n+  mavlink_message_t message;\n+  if (serial_port_open_ && nh_.ok()) {\n+    int res = sp_.read_message(message);\n+    if (res == -1) {\n+      serial_port_open_ = false;\n+      return;\n+    }\n+\n+    mavlink_heartbeat_t heartbeat;\n+\n+    while (heartbeat.type != 9) {  // check for system type 9 heartbeat\n+\n+      if (heartbeat.type != 9) {\n+        printf(\".\");\n+        fflush(stdout);\n+        usleep(50000);\n+      }\n+      if (message.msgid == 0) {\n+        mavlink_msg_heartbeat_decode(&message, &heartbeat);\n+        if (heartbeat.type == 9) {\n+          std::cout << std::endl;\n+          ROS_INFO(\"Got heartbeat from camera\");\n+        }\n+      }\n+      if (serial_port_open_ && nh_.ok()) {\n+        int res = sp_.read_message(message);\n+        if (res == -1) {\n+          serial_port_open_ = false;\n+          return;\n         }\n+      }\n+    }\n \n-\t//sp_.open_serial();\n-\n-\t// initialize camera image publisher\n-\tswitch (n_cameras_) {\n-\tcase 10:\n-\t\tcam_9_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_9/image_raw\", 5);\n-\t\tcam_9_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_9/camera_info\", 5);\n-\t\tcam_8c_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_8/image_rect\", 5);\n-\t\tcam_8c_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_8/camera_info\", 5);\n-\t\tcam_8d_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_8/image_depth\", 5);\n-\t\tcam_8d_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_8/camera_info\", 5);\n-\t\t// initialize imu msg publisher\n-\t\timu8_publisher_ = nh_.advertise<sensor_msgs::Imu>(\"cam_8/imu\", 20);\n-\t\timu9_publisher_ = nh_.advertise<sensor_msgs::Imu>(\"cam_9/imu\", 20);\n-\n-\tcase 9:\n-\t\tcam_8_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_8/image_raw\", 5);\n-\t\tcam_8_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_8/camera_info\", 5);\n-\n-\tcase 8:\n-\t\tcam_7_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_7/image_raw\", 5);\n-\t\tcam_7_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_7/camera_info\", 5);\n-\t\tcam_6c_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_6/image_rect\", 5);\n-\t\tcam_6c_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_6/camera_info\", 5);\n-\t\tcam_6d_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_6/image_depth\", 5);\n-\t\tcam_6d_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_6/camera_info\", 5);\n-\t\t// initialize imu msg publisher\n-\t\timu6_publisher_ = nh_.advertise<sensor_msgs::Imu>(\"cam_6/imu\", 20);\n-\t\timu7_publisher_ = nh_.advertise<sensor_msgs::Imu>(\"cam_7/imu\", 20);\n-\n-\tcase 7:\n-\t\tcam_6_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_6/image_raw\", 5);\n-\t\tcam_6_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_6/camera_info\", 5);\n-\n-\tcase 6:\n-\t\tcam_5_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_5/image_raw\", 5);\n-\t\tcam_5_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_5/camera_info\", 5);\n-\t\tcam_4c_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_4/image_rect\", 5);\n-\t\tcam_4c_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_4/camera_info\", 5);\n-\t\tcam_4d_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_4/image_depth\", 5);\n-\t\tcam_4d_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_4/camera_info\", 5);\n-\t\t// initialize imu msg publisher\n-\t\timu4_publisher_ = nh_.advertise<sensor_msgs::Imu>(\"cam_4/imu\", 20);\n-\t\timu5_publisher_ = nh_.advertise<sensor_msgs::Imu>(\"cam_5/imu\", 20);\n-\n-\tcase 5:\n-\t\tcam_4_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_4/image_raw\", 5);\n-\t\tcam_4_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_4/camera_info\", 5);\n-\n-\tcase 4:\n-\t\tcam_3_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_3/image_raw\", 5);\n-\t\tcam_3_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_3/camera_info\", 5);\n-\t\tcam_2c_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_2/image_rect\", 5);\n-\t\tcam_2c_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_2/camera_info\", 5);\n-\t\tcam_2d_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_2/image_depth\", 5);\n-\t\tcam_2d_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_2/camera_info\", 5);\n-\t\t// initialize imu msg publisher\n-\t\timu2_publisher_ = nh_.advertise<sensor_msgs::Imu>(\"cam_2/imu\", 20);\n-\t\timu3_publisher_ = nh_.advertise<sensor_msgs::Imu>(\"cam_3/imu\", 20);\n-\n-\tcase 3:\n-\t\tcam_2_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_2/image_raw\", 5);\n-\t\tcam_2_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_2/camera_info\", 5);\n-\n-\tcase 2:\n-\t\tcam_1_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_1/image_raw\", 5);\n-\t\tcam_1_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_1/camera_info\", 5);\n-\t\tcam_0c_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_0/image_rect\", 5);\n-\t\tcam_0c_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_0/camera_info\", 5);\n-\t\tcam_0d_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_0/image_depth\", 5);\n-\t\tcam_0d_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_0/camera_info\", 5);\n-\t\t// initialize imu msg publisher\n-\t\timu0_publisher_ = nh_.advertise<sensor_msgs::Imu>(\"cam_0/imu\", 20);\n-\t\timu1_publisher_ = nh_.advertise<sensor_msgs::Imu>(\"cam_1/imu\", 20);\n-\n-\tdefault:\n-\t\tcam_0_pub_ = nh_.advertise<sensor_msgs::Image>(\n-\t\t\t\t     node_name_ + \"/cam_0/image_raw\", 5);\n-\t\tcam_0_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(\n-\t\t\t\t\t  node_name_ + \"/cam_0/camera_info\", 5);\n-\t}\n-\n-\n-\t// wait on heart beat\n-\tstd::cout << \"Waiting on device.\";\n-\tfflush(stdout);\n-\tmavlink_message_t message;\n-\tif(serial_port_open_ && nh_.ok()){\n-\t\tint res = sp_.read_message(message);\n-\t\tif(res == -1){\n-\t\t\tserial_port_open_ = false;\n-\t\t\treturn;\n-\t\t}\n-\n-\tmavlink_heartbeat_t heartbeat;\n-\n-\twhile (heartbeat.type !=9){ //check for system type 9 heartbeat\n-\n-\t\tif(heartbeat.type!=9){\n-\t\t\tprintf(\".\");\n-\t\t\tfflush(stdout);\n-\t\t\tusleep(50000);\n-\t\t}\n-\t\tif(message.msgid ==0 ){\n-\t\t\tmavlink_msg_heartbeat_decode(&message, &heartbeat);\n-\t\t\tif(heartbeat.type ==9){\n-\t\t\t\tstd::cout << std::endl;\n-\t\t\t\tROS_INFO(\"Got heartbeat from camera\");\n-\t\t\t}\n-\t\t}\n-\t\tif(serial_port_open_ && nh_.ok()){\n-\t\t\tint res = sp_.read_message(message);\n-\t\t\tif(res == -1){\n-\t\t\t\tserial_port_open_ = false;\n-\t\t\t\treturn;\n-\t\t\t}\n-\t\t}\n-\t}\n-\n-\t//std::cout  << \"Device connected\" << std::endl;\n-\t//setCalibration(camera_params_);\n-\t// set flag for completed initializiation\n-\tdevice_initialized_ = true;\n-\t}\n+    // set flag for completed initializiation\n+    device_initialized_ = true;\n+  }\n }\n \n ////////////////////////////////////////////////////////////////////////////////\n \n-void uvcROSDriver::startDevice()\n-{\n-\tif (device_initialized_) {\n-\n-\t\tsetCalibration(camera_params_);\n-\n-\t\t// open uvc stream\n-\t\tuvc_error_t res = initAndOpenUvc();\n-\t\t// start stream\n-\t\tpast_ = ros::Time::now();\n-\t\tres = uvc_start_streaming(devh_, &ctrl_, &callback, this, 0);\n-\t\t\t//mavlink_param_value_t param;\n-\t\t\t//bool wait=1;\n-\t\t\t//std::string str = \"CAMERA_ENABLE\";\n-\t\tsetParam(\"CAMERA_ENABLE\",float(camera_config_));\n-\t\t\t//printf(\"Wait on camera \\n\");\n-\t\t//usleep(200000);\n-\n-\t\t/*while(!uvc_cb_flag_ && ros::ok()){\n-\t\t\t\t/*mavlink_message_t message;\n-\t\t\t\tint res = sp_.read_message(message);\n-\t\t\t\tif(res != 0){\n-\t\t\t\t\tif(message.msgid==22){\n-\t\t\t\t\t\t//printf(\"got message id 22...\\n\");\n-\t\t\t\t\t\tmavlink_msg_param_value_decode(&message, &param);\n-\t\t\t\t\t\tif(str.compare(param.param_id)==0 && param.param_value==float(camera_config_)){\n-\t\t\t\t\t\t\twait = 0;\n-\t\t\t\t\t\t\tprintf(\"camera started\\n\");\n-\t\t\t\t\t\t}\n-\t\t\t\t\t\telse{\n-\t\t\t\t\t\t\tsetParam(\"CAMERA_ENABLE\",float(camera_config_));\n-\t\t\t\t\t\t\tstd::cout << \".\" ;\n-\t\t\t\t\t\t}\n-\t\t\t\t\t}\n-\n-\t\t\t\t}*/\n-\t\t\t\t//std::cout << \".\" ;\n-\t\t\t/*setParam(\"CAMERA_ENABLE\",float(camera_config_));\n-\t\t\tusleep(200000);\n-\t\t}*/\n-\n-\t\t\t//printf(\" done\\n\");\n-\t\tprintf(\"Waiting on stream\");\n-\t\twhile (!uvc_cb_flag_ && ros::ok()) {\n-\t\t\tprintf(\".\");\n-\t\t\tfflush(stdout);\n-\t\t\t//uvc_stop_streaming(devh_);\n-\t\t\t//res = uvc_start_streaming(devh_, &ctrl_, &callback, this, 0);\n-\t\t\t\n-\t\t\tif(setParam(\"CAMERA_ENABLE\",float(camera_config_))==-1){\n-\t\t\t\tROS_ERROR(\"Device not initialized!\");\n-\t\t\t\treturn;\n-\t\t\t}\n-\t\t\tusleep(200000);\n-\t\t\t// std::cout << \"res: \" << res << std::endl;\n-\t\t}\n-\n-\t} else {\n-\t\tROS_ERROR(\"Device not initialized!\");\n-\t}\n+void uvcROSDriver::startDevice() {\n+  if (device_initialized_) {\n+    setCalibration(camera_params_);\n+\n+    // open uvc stream\n+    uvc_error_t res = initAndOpenUvc();\n+    if (res < 0) {\n+      uvc_perror(res, \"uvc_init\");\n+      ROS_ERROR(\"Unable to open uvc device\");\n+      return;\n+    }\n+\n+    // start stream\n+    res = uvc_start_streaming(devh_, &ctrl_, &callback, this, 0);\n+\n+    setParam(\"CAMERA_ENABLE\", static_cast<float>(buildCameraConfig()));\n+    setCalibration(camera_params_);\n+\n+    printf(\"Waiting on stream\");\n+    while (!uvc_cb_flag_ && ros::ok()) {\n+      printf(\".\");\n+      fflush(stdout);\n+\n+      if (setParam(\"CAMERA_ENABLE\", static_cast<float>(buildCameraConfig())) == -1) {\n+        ROS_ERROR(\"Device not initialized!\");\n+        return;\n+      }\n+      usleep(200000);\n+    }\n+\n+    ROS_INFO(\"Enabled Dynamic Reconfigure Callback\");\n+    dynamic_reconfigure_.setCallback(\n+        std::bind(&uvcROSDriver::dynamicReconfigureCallback, this,\n+                  std::placeholders::_1, std::placeholders::_2));\n+\n+  } else {\n+    ROS_ERROR(\"Device not initialized!\");\n+  }\n }\n \n ////////////////////////////////////////////////////////////////////////////////\n \n-int uvcROSDriver::setParam(const std::string &name, float val)\n-{\n-\tmavlink_message_t msg;\n-\tchar name_buf[16] = {};\n+int uvcROSDriver::setParam(const std::string &name, float val) {\n+  mavlink_message_t msg;\n+  char name_buf[16] = {};\n \n-\tuint8_t local_sys = 1;\n-\tuint8_t local_comp = 1;\n+  uint8_t local_sys = 1;\n+  uint8_t local_comp = 1;\n \n-\tuint8_t target_sys = 99;\n-\tuint8_t target_comp = 55;\n+  uint8_t target_sys = 99;\n+  uint8_t target_comp = 55;\n \n-\t// SETCALIB\n-\tstrncpy(name_buf, name.c_str(), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);\n-\tmavlink_msg_param_set_pack(local_sys, local_comp, &msg, target_sys,\n-\t\t\t\t   target_comp, name_buf, val, MAVLINK_TYPE_FLOAT);\n-\tint ret = sp_.write_message(msg);\n+  constexpr size_t kAttempts = 2;\n \n-\tif (ret <= 0) {\n-\t\tprintf(\"ret: %d\\n\", ret);\n-\t\treturn ret;\n-\t}\n+  // SETCALIB\n+  // multiple attempts, just so we maximize chances things actually go through\n+  for (size_t i = 0; i < kAttempts; ++i) {\n+    strncpy(name_buf, name.c_str(), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);\n+    mavlink_msg_param_set_pack(local_sys, local_comp, &msg, target_sys,\n+                               target_comp, name_buf, val, MAVLINK_TYPE_FLOAT);\n+    int ret = sp_.write_message(msg);\n \n-\t// another time, just so we maximize chances things actually go through\n-\tstrncpy(name_buf, name.c_str(), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);\n-\tmavlink_msg_param_set_pack(local_sys, local_comp, &msg, target_sys,\n-\t\t\t\t   target_comp, name_buf, val, MAVLINK_TYPE_FLOAT);\n-\tret = sp_.write_message(msg);\n+    if (ret <= 0) {\n+      printf(\"ret: %d\\n\", ret);\n+      return ret;\n+    }\n+  }\n \n-\tif (ret <= 0) {\n-\t\tprintf(\"ret: %d\\n\", ret);\n-\t\treturn ret;\n-\t}\n-\n-\treturn 0;\n+  return 0;\n }\n \n ////////////////////////////////////////////////////////////////////////////////\n \n-void uvcROSDriver::sendCameraParam(const int camera_number, const double fx,\n-\t\t\t\t   const double fy, const Eigen::Vector2d &p0,\n-\t\t\t\t   const float k1, const float k2,\n-\t\t\t\t   const float r1, const float r2,\n-\t\t\t\t   const Eigen::Matrix3d &H)\n-{\n-\tstd::string camera_name = \"CAM\" + std::to_string(camera_number);\n-\tsetParam(\"PARAM_CCX_\" + camera_name, p0[0]);\n-\tsetParam(\"PARAM_CCY_\" + camera_name, p0[1]);\n-\tsetParam(\"PARAM_FCX_\" + camera_name, fx);\n-\tsetParam(\"PARAM_FCY_\" + camera_name, fy);\n-\tsetParam(\"PARAM_KC1_\" + camera_name, k1);\n-\tsetParam(\"PARAM_KC2_\" + camera_name, k2);\n-\tsetParam(\"PARAM_P1_\" + camera_name, r1);\n-\tsetParam(\"PARAM_P2_\" + camera_name, r2);\n-\tsetParam(\"PARAM_H11_\" + camera_name, H(0, 0));\n-\tsetParam(\"PARAM_H12_\" + camera_name, H(0, 1));\n-\tsetParam(\"PARAM_H13_\" + camera_name, H(0, 2));\n-\tsetParam(\"PARAM_H21_\" + camera_name, H(1, 0));\n-\tsetParam(\"PARAM_H22_\" + camera_name, H(1, 1));\n-\tsetParam(\"PARAM_H23_\" + camera_name, H(1, 2));\n-\tsetParam(\"PARAM_H31_\" + camera_name, H(2, 0));\n-\tsetParam(\"PARAM_H32_\" + camera_name, H(2, 1));\n-\tsetParam(\"PARAM_H33_\" + camera_name, H(2, 2));\n+void uvcROSDriver::sendCameraParam(\n+    const int camera_number, const uvc_ros_driver::DistortionModelTypes dtype,\n+    const double fx, const double fy, const Eigen::Vector2d &p0, const float k1,\n+    const float k2, const float r1, const float r2, const Eigen::Matrix3d &H) {\n+  std::string camera_name = \"CAM\" + std::to_string(camera_number);\n+\n+  setParam(\"PARAM_DM_\" + camera_name, static_cast<int>(dtype));\n+  setParam(\"PARAM_CCX_\" + camera_name, p0[0]);\n+  setParam(\"PARAM_CCY_\" + camera_name, p0[1]);\n+  setParam(\"PARAM_FCX_\" + camera_name, fx);\n+  setParam(\"PARAM_FCY_\" + camera_name, fy);\n+  setParam(\"PARAM_KC1_\" + camera_name, k1);\n+  setParam(\"PARAM_KC2_\" + camera_name, k2);\n+  setParam(\"PARAM_KC3_\" + camera_name, r1);\n+  setParam(\"PARAM_KC4_\" + camera_name, r2);\n+  setParam(\"PARAM_H11_\" + camera_name, H(0, 0));\n+  setParam(\"PARAM_H12_\" + camera_name, H(0, 1));\n+  setParam(\"PARAM_H13_\" + camera_name, H(0, 2));\n+  setParam(\"PARAM_H21_\" + camera_name, H(1, 0));\n+  setParam(\"PARAM_H22_\" + camera_name, H(1, 1));\n+  setParam(\"PARAM_H23_\" + camera_name, H(1, 2));\n+  setParam(\"PARAM_H31_\" + camera_name, H(2, 0));\n+  setParam(\"PARAM_H32_\" + camera_name, H(2, 1));\n+  setParam(\"PARAM_H33_\" + camera_name, H(2, 2));\n }\n \n ////////////////////////////////////////////////////////////////////////////////\n \n-void uvcROSDriver::setCalibration(CameraParameters camParams)\n-{\n-\t// uvc_ros_driver::FPGACalibration cams[n_cameras_];\n-\tstd::vector<uvc_ros_driver::FPGACalibration> cams;\n-\tint stereo_number = 0;\n-\n-\tif (camParams.isValid) {\n-\t\t// TODO: find better way for this, general case not only stereo between\n-\t\t// cam0->cam1, cam2->cam3\n-\t\tfor (int cam = 0; cam < n_cameras_; cam++) {\n-\t\t\tuvc_ros_driver::FPGACalibration camera;\n-\t\t\tcamera.projection_model_.type_ =\n-\t\t\t\tuvc_ros_driver::ProjectionModelTypes::PINHOLE;\n-\t\t\tcamera.projection_model_.focal_length_u_ = camParams.FocalLength[cam][0];\n-\t\t\tcamera.projection_model_.focal_length_v_ = camParams.FocalLength[cam][1];\n-\t\t\tcamera.projection_model_.principal_point_u_ =\n-\t\t\t\tcamParams.PrincipalPoint[cam][0];\n-\t\t\tcamera.projection_model_.principal_point_v_ =\n-\t\t\t\tcamParams.PrincipalPoint[cam][1];\n-\t\t\tcamera.projection_model_.k1_ = camParams.DistortionCoeffs[cam][0];\n-\t\t\tcamera.projection_model_.k2_ = camParams.DistortionCoeffs[cam][1];\n-\t\t\tcamera.projection_model_.r1_ = camParams.DistortionCoeffs[cam][2];\n-\t\t\tcamera.projection_model_.r2_ = camParams.DistortionCoeffs[cam][3];\n-\n-\t\t\tif (cam % 2 != 0) {\n-\t\t\t\tcamera.projection_model_.R_[0] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][0][0];\n-\t\t\t\tcamera.projection_model_.R_[1] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][0][1];\n-\t\t\t\tcamera.projection_model_.R_[2] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][0][2];\n-\t\t\t\tcamera.projection_model_.R_[3] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][1][0];\n-\t\t\t\tcamera.projection_model_.R_[4] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][1][1];\n-\t\t\t\tcamera.projection_model_.R_[5] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][1][2];\n-\t\t\t\tcamera.projection_model_.R_[6] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][2][0];\n-\t\t\t\tcamera.projection_model_.R_[7] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][2][1];\n-\t\t\t\tcamera.projection_model_.R_[8] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][2][2];\n-\t\t\t\tcamera.projection_model_.t_[0] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][0][3];\n-\t\t\t\tcamera.projection_model_.t_[1] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][1][3];\n-\t\t\t\tcamera.projection_model_.t_[2] =\n-\t\t\t\t\tcamParams.StereoTransformationMatrix[stereo_number][2][3];\n-\t\t\t\tstereo_number++;\n-\n-\t\t\t} else {\n-\t\t\t\tcamera.projection_model_.R_[0] = 1.0f;\n-\t\t\t\tcamera.projection_model_.R_[1] = 0.0f;\n-\t\t\t\tcamera.projection_model_.R_[2] = 0.0f;\n-\t\t\t\tcamera.projection_model_.R_[3] = 0.0f;\n-\t\t\t\tcamera.projection_model_.R_[4] = 1.0f;\n-\t\t\t\tcamera.projection_model_.R_[5] = 0.0f;\n-\t\t\t\tcamera.projection_model_.R_[6] = 0.0f;\n-\t\t\t\tcamera.projection_model_.R_[7] = 0.0f;\n-\t\t\t\tcamera.projection_model_.R_[8] = 1.0f;\n-\t\t\t\tcamera.projection_model_.t_[0] = 0.0f;\n-\t\t\t\tcamera.projection_model_.t_[1] = 0.0f;\n-\t\t\t\tcamera.projection_model_.t_[2] = 0.0f;\n-\t\t\t}\n-\n-\t\t\tcams.push_back(camera);\n-\t\t}\n-\n-\t\t// initialize vectors\n-\t\tf_.resize(n_cameras_);\n-\t\tp_.resize(n_cameras_);\n-\t\tH_.resize(n_cameras_);\n-\t\t// pointer at camera info\n-\t\tsensor_msgs::CameraInfo *ci;\n-\t\tsize_t homography_size;\n-\n-\t\t// TODO: reimplment this part for multiple stereo base line based systems\n-\t\tif (set_calibration_) {\n-\t\t\tstd::vector<std::pair<int, int> >::iterator it_homography =\n-\t\t\t\tstd::max_element(homography_mapping_.begin(), homography_mapping_.end(),\n-\t\t\t\t\t\t myPairMax);\n-\t\t\t/*if ((*it_homography).second > n_cameras_) {\n-\t\t\t\thomography_size = std::distance(homography_mapping_.begin(), it_homography);\n-\n-\t\t\t} else {\n-\t\t\t\thomography_size = homography_mapping_.size();\n-\t\t\t}*/\n-\t\t\t//set homography number to number of camera pairs for now\n-\t\t\thomography_size = n_cameras_/2;\n-\n-\t\t\tfor (size_t i = 0; i < homography_size; i++) {\n-\t\t\t\t// temp structures\n-\t\t\t\tEigen::Matrix3d H0;\n-\t\t\t\tEigen::Matrix3d H1;\n-\t\t\t\tdouble f_new;\n-\t\t\t\tEigen::Vector2d p0_new, p1_new;\n-\n-\t\t\t\tstd::pair<int, int> indx = homography_mapping_[i];\n-\n-\t\t\t\tStereoHomography h(cams[indx.first], cams[indx.second]);\n-\t\t\t\th.getHomography(H0, H1, f_new, p0_new, p1_new);\n-\n-\t\t\t\tf_[indx.first] = f_new;\n-\t\t\t\tf_[indx.second] = f_new;\n-\t\t\t\tp_[indx.first] = p0_new;\n-\t\t\t\tp_[indx.second] = p1_new;\n-\t\t\t\t// TODO check if matrix is copied or only pointer!!\n-\t\t\t\tH_[indx.first] = H0;\n-\t\t\t\tH_[indx.second] = H1;\n-\t\t\t}\n-\n-\t\t\t// Set all parameters here\n-\t\t\tfor (int i = 0; i < n_cameras_; i++) {\n-\t\t\t\tsendCameraParam(i, f_[i], f_[i], p_[i], cams[i].projection_model_.k1_,\n-\t\t\t\t\t\tcams[i].projection_model_.k2_,\n-\t\t\t\t\t\tcams[i].projection_model_.r1_,\n-\t\t\t\t\t\tcams[i].projection_model_.r2_, H_[i]);\n-\t\t\t\tselectCameraInfo(i, &ci);\n-\t\t\t\tsetCameraInfoIntrinsics(*ci, f_[i], f_[i], p_[i](0), p_[i](1));\n-\t\t\t\tsetCameraInfoDistortionMdl(\n-\t\t\t\t\t*ci, uvc_ros_driver::ProjectionModelTypes::PINHOLE);\n-\t\t\t\tsetCameraInfoDistortionParams(*ci, 0, 0, 0, 0, 0);\n-\t\t\t}\n-\n-\t\t} else {\n-\t\t\tfor (int i = 0; i < n_cameras_; i++) {\n-\t\t\t\tselectCameraInfo(i, &ci);\n-\t\t\t\tsetCameraInfoIntrinsics(\n-\t\t\t\t\t*ci, camParams.FocalLength[i][0], camParams.FocalLength[i][1],\n-\t\t\t\t\tcamParams.PrincipalPoint[i][0], camParams.PrincipalPoint[i][1]);\n-\t\t\t\tsetCameraInfoDistortionMdl(\n-\t\t\t\t\t*ci, uvc_ros_driver::ProjectionModelTypes::PINHOLE);\n-\t\t\t\tsetCameraInfoDistortionParams(\n-\t\t\t\t\t*ci, cams[i].projection_model_.k1_, cams[i].projection_model_.k2_,\n-\t\t\t\t\tcams[i].projection_model_.r1_, cams[i].projection_model_.r2_, 0);\n-\t\t\t}\n-\t\t}\n-\n-\n-\t\t// TODO: implement with class variables\n-\t\t// SGM stereo penalty values p1: discontinuits, p2:\n-\t\tsetParam(\"STEREO_P1_CAM1\", 10.0f);\n-\t\tsetParam(\"STEREO_P2_CAM1\", 250.0f);\n-\t\t// disparity L->R occlusion in px\n-\t\tsetParam(\"STEREO_LR_CAM1\", 3.0f);\n-\t\t// threshold 0-255 valid disparity\n-\t\tsetParam(\"STEREO_TH_CAM1\", 140.0f);\n-\t\tsetParam(\"STEREO_FP_CAM1\", 0.0f);\n-\t\tsetParam(\"STEREO_CE_CAM1\", 0.0f);\n-\t\tsetParam(\"STEREO_RE_CAM1\", 0.0f);\n-\t\tsetParam(\"STEREO_OF_CAM1\", 0.0f);\n-\n-\t\t//setParam(\"COST_SHIFT\", 2.0f);\n-\n-\t\t//setParam(\"CAMERA_AUTOEXP\",0.0f);\n-\t\t//setParam(\"CAMERA_EXP\",480.0f);\n-\t\t//setParam(\"CAMERA_AUTOG\",0.0f);\n-\t\t//setParam(\"CAMERA_GAIN\",63.0f);\n-\n-\t\tsetParam(\"STEREO_MP_01\", 0.0f);\n-\t\tsetParam(\"STEREO_BAYER_D\", 0.0f);\n-\t\tsetParam(\"IMU_ENABLE\",(float)n_cameras_);\n-\t\tsetParam(\"ADIS_IMU\",0.0f);\n-\n-\t\tsetParam(\"STEREO_P1_CAM3\", 10.0f);\n-\t\tsetParam(\"STEREO_P2_CAM3\", 250.0f);\n-\t\tsetParam(\"STEREO_LR_CAM3\", 3.0f);\n-\t\tsetParam(\"STEREO_TH_CAM3\", 140.0f);\n-\t\tsetParam(\"STEREO_FP_CAM3\", 0.0f);\n-\t\tsetParam(\"STEREO_CE_CAM3\", 0.0f);\n-\t\tsetParam(\"STEREO_RE_CAM3\", 0.0f);\n-\t\tsetParam(\"STEREO_OF_CAM3\", 0.0f);\n-\n-\t\tsetParam(\"STEREO_P1_CAM5\", 16.0f);\n-\t\tsetParam(\"STEREO_P2_CAM5\", 240.0f);\n-\t\tsetParam(\"STEREO_LR_CAM5\", 4.0f);\n-\t\tsetParam(\"STEREO_TH_CAM5\", 120.0f);\n-\t\tsetParam(\"STEREO_FP_CAM5\", 0.0f);\n-\t\tsetParam(\"STEREO_CE_CAM5\", 0.0f);\n-\t\tsetParam(\"STEREO_RE_CAM5\", 0.0f);\n-\t\tsetParam(\"STEREO_OF_CAM5\", 0.0f);\n-\n-\t\tsetParam(\"STEREO_P1_CAM7\", 16.0f);\n-\t\tsetParam(\"STEREO_P2_CAM7\", 240.0f);\n-\t\tsetParam(\"STEREO_LR_CAM7\", 4.0f);\n-\t\tsetParam(\"STEREO_TH_CAM7\", 120.0f);\n-\t\tsetParam(\"STEREO_FP_CAM7\", 0.0f);\n-\t\tsetParam(\"STEREO_CE_CAM7\", 0.0f);\n-\t\tsetParam(\"STEREO_RE_CAM7\", 0.0f);\n-\t\tsetParam(\"STEREO_OF_CAM7\", 0.0f);\n-\n-\t\tsetParam(\"STEREO_P1_CAM9\", 16.0f);\n-\t\tsetParam(\"STEREO_P2_CAM9\", 240.0f);\n-\t\tsetParam(\"STEREO_LR_CAM9\", 4.0f);\n-\t\tsetParam(\"STEREO_TH_CAM9\", 120.0f);\n-\t\tsetParam(\"STEREO_FP_CAM9\", 0.0f);\n-\t\tsetParam(\"STEREO_CE_CAM9\", 0.0f);\n-\t\tsetParam(\"STEREO_RE_CAM9\", 0.0f);\n-\t\tsetParam(\"STEREO_OF_CAM9\", 0.0f);\n-\n-\t\tsetParam(\"CALIB_GAIN\", 4300.0f);\n-\n-\t\tsetParam(\"CAMERA_H_FLIP\", float(flip_));\n-\n-\t\tif(flip_){\n-\t\t\tsetParam(\"IM_H_FLIP_CAM0\", 0.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM0\", 0.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM2\", 0.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM2\", 0.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM4\", 0.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM4\", 0.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM6\", 0.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM6\", 0.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM8\", 0.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM8\", 0.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM1\", 1.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM1\", 1.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM3\", 1.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM3\", 1.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM5\", 1.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM5\", 1.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM7\", 1.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM7\", 1.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM9\", 1.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM9\", 1.0f);\n-\t\t}\n-\t\telse{\n-\t\t\tsetParam(\"IM_H_FLIP_CAM0\", 1.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM0\", 1.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM2\", 1.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM2\", 1.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM4\", 1.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM4\", 1.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM6\", 1.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM6\", 1.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM8\", 1.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM8\", 1.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM1\", 0.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM1\", 0.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM3\", 0.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM3\", 0.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM5\", 0.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM5\", 0.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM7\", 0.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM7\", 0.0f);\n-\t\t\tsetParam(\"IM_H_FLIP_CAM9\", 0.0f);\n-\t\t\tsetParam(\"IM_V_FLIP_CAM9\", 0.0f);\n-\t\t}\n-\n-\t}\n-\n-\t/*if (set_calibration_) {\n-\t\tsetParam(\"RESETCALIB\", 0.0f);\n-\n-\t} else {\n-\t\tsetParam(\"RESETCALIB\", 1.0f);\n-\t}*/\n-\n-\n-\tsetParam(\"SETCALIB\", float(set_calibration_));\n-\n-\t// std::cout << \"Configuring cameras...\" << std::endl;\n-\tsetParam(\"RESETMT9V034\", 1.0f);\n-\tsetParam(\"RESETICM20608\",1.0f);\n-\t// sleep(5);  // needed, fpga reconfigure cameras and restart time\n-\t// std::cout << \"Configuration completed.\" << std::endl;\n-\t// last 4 bits activate the 4 camera pairs 0x01 = pair 1 only, 0x0F all 4\n-\t// pairs\n-\t//setParam(\"CAMERA_ENABLE\", float(camera_config_));\n+void uvcROSDriver::setCalibration(CameraParameters camParams) {\n+  std::vector<uvc_ros_driver::FPGACalibration> cams;\n+  int stereo_number = 0;\n+\n+  if (camParams.isValid) {\n+    // TODO: find better way for this, general case not only stereo between\n+    // cam0->cam1, cam2->cam3\n+    for (int cam = 0; cam < n_cameras_; cam++) {\n+      uvc_ros_driver::FPGACalibration camera;\n+      camera.projection_model_.projection_type_ =\n+          uvc_ros_driver::ProjectionModelTypes::PINHOLE;\n+      camera.projection_model_.distortion_type_ =\n+          static_cast<uvc_ros_driver::DistortionModelTypes>(\n+              camParams.DistortionModel[cam]);\n+      camera.projection_model_.focal_length_u_ = camParams.FocalLength[cam][0];\n+      camera.projection_model_.focal_length_v_ = camParams.FocalLength[cam][1];\n+      camera.projection_model_.principal_point_u_ =\n+          camParams.PrincipalPoint[cam][0];\n+      camera.projection_model_.principal_point_v_ =\n+          camParams.PrincipalPoint[cam][1];\n+      camera.projection_model_.k1_ = camParams.DistortionCoeffs[cam][0];\n+      camera.projection_model_.k2_ = camParams.DistortionCoeffs[cam][1];\n+      camera.projection_model_.r1_ = camParams.DistortionCoeffs[cam][2];\n+      camera.projection_model_.r2_ = camParams.DistortionCoeffs[cam][3];\n+\n+      if (cam % 2 != 0) {\n+        camera.projection_model_.R_[0] =\n+            camParams.StereoTransformationMatrix[stereo_number][0][0];\n+        camera.projection_model_.R_[1] =\n+            camParams.StereoTransformationMatrix[stereo_number][0][1];\n+        camera.projection_model_.R_[2] =\n+            camParams.StereoTransformationMatrix[stereo_number][0][2];\n+        camera.projection_model_.R_[3] =\n+            camParams.StereoTransformationMatrix[stereo_number][1][0];\n+        camera.projection_model_.R_[4] =\n+            camParams.StereoTransformationMatrix[stereo_number][1][1];\n+        camera.projection_model_.R_[5] =\n+            camParams.StereoTransformationMatrix[stereo_number][1][2];\n+        camera.projection_model_.R_[6] =\n+            camParams.StereoTransformationMatrix[stereo_number][2][0];\n+        camera.projection_model_.R_[7] =\n+            camParams.StereoTransformationMatrix[stereo_number][2][1];\n+        camera.projection_model_.R_[8] =\n+            camParams.StereoTransformationMatrix[stereo_number][2][2];\n+        camera.projection_model_.t_[0] =\n+            camParams.StereoTransformationMatrix[stereo_number][0][3];\n+        camera.projection_model_.t_[1] =\n+            camParams.StereoTransformationMatrix[stereo_number][1][3];\n+        camera.projection_model_.t_[2] =\n+            camParams.StereoTransformationMatrix[stereo_number][2][3];\n+        stereo_number++;\n+\n+      } else {\n+        camera.projection_model_.R_[0] = 1.0f;\n+        camera.projection_model_.R_[1] = 0.0f;\n+        camera.projection_model_.R_[2] = 0.0f;\n+        camera.projection_model_.R_[3] = 0.0f;\n+        camera.projection_model_.R_[4] = 1.0f;\n+        camera.projection_model_.R_[5] = 0.0f;\n+        camera.projection_model_.R_[6] = 0.0f;\n+        camera.projection_model_.R_[7] = 0.0f;\n+        camera.projection_model_.R_[8] = 1.0f;\n+        camera.projection_model_.t_[0] = 0.0f;\n+        camera.projection_model_.t_[1] = 0.0f;\n+        camera.projection_model_.t_[2] = 0.0f;\n+      }\n+\n+      cams.push_back(camera);\n+    }\n+\n+    // initialize vectors\n+    f_.resize(n_cameras_);\n+    p_.resize(n_cameras_);\n+    H_.resize(n_cameras_);\n+    // pointer at camera info\n+    sensor_msgs::CameraInfo *ci;\n+    size_t homography_size;\n+\n+    // TODO: reimplment this part for multiple stereo base line based systems\n+    if (set_calibration_) {\n+      std::vector<std::pair<int, int> >::iterator it_homography =\n+          std::max_element(homography_mapping_.begin(),\n+                           homography_mapping_.end(), myPairMax);\n+\n+      // set homography number to number of camera pairs for now\n+      homography_size = n_cameras_ / 2;\n+\n+      for (size_t i = 0; i < homography_size; i++) {\n+        // temp structures\n+        Eigen::Matrix3d H0;\n+        Eigen::Matrix3d H1;\n+        double f_new;\n+        Eigen::Vector2d p0_new, p1_new;\n+\n+        std::pair<int, int> indx = homography_mapping_[i];\n+\n+        StereoHomography h(cams[indx.first], cams[indx.second]);\n+        h.getHomography(H0, H1, f_new, p0_new, p1_new);\n+\n+        f_[indx.first] = f_new;\n+        f_[indx.second] = f_new;\n+        p_[indx.first] = p0_new;\n+        p_[indx.second] = p1_new;\n+        // TODO check if matrix is copied or only pointer!!\n+        H_[indx.first] = H0;\n+        H_[indx.second] = H1;\n+      }\n+\n+      // Set all parameters here\n+      for (int i = 0; i < n_cameras_; i++) {\n+        sendCameraParam(i, cams[i].projection_model_.distortion_type_, f_[i],\n+                        f_[i], p_[i], cams[i].projection_model_.k1_,\n+                        cams[i].projection_model_.k2_,\n+                        cams[i].projection_model_.r1_,\n+                        cams[i].projection_model_.r2_, H_[i]);\n+        setCameraInfoIntrinsics(info_cams_[i], f_[i], f_[i], p_[i](0),\n+                                p_[i](1));\n+        setCameraInfoDistortionMdl(\n+            info_cams_[i], uvc_ros_driver::ProjectionModelTypes::PINHOLE);\n+        setCameraInfoDistortionParams(info_cams_[i], 0, 0, 0, 0, 0);\n+      }\n+\n+    } else {\n+      for (int i = 0; i < n_cameras_; i++) {\n+        setCameraInfoIntrinsics(info_cams_[i], camParams.FocalLength[i][0],\n+                                camParams.FocalLength[i][1],\n+                                camParams.PrincipalPoint[i][0],\n+                                camParams.PrincipalPoint[i][1]);\n+        setCameraInfoDistortionMdl(\n+            info_cams_[i], uvc_ros_driver::ProjectionModelTypes::PINHOLE);\n+        setCameraInfoDistortionParams(\n+            info_cams_[i], cams[i].projection_model_.k1_,\n+            cams[i].projection_model_.k2_, cams[i].projection_model_.r1_,\n+            cams[i].projection_model_.r2_, 0);\n+      }\n+    }\n+\n+    // TODO: implement with class variables\n+    // SGM stereo penalty values p1: discontinuits, p2:\n+    setParam(\"STEREO_P1_CAM1\", 10.0f);\n+    setParam(\"STEREO_P2_CAM1\", 250.0f);\n+    // disparity L->R occlusion in px\n+    setParam(\"STEREO_LR_CAM1\", 3.0f);\n+    // threshold 0-255 valid disparity\n+    setParam(\"STEREO_TH_CAM1\", 140.0f);\n+    setParam(\"STEREO_FP_CAM1\", 0.0f);\n+    setParam(\"STEREO_CE_CAM1\", 0.0f);\n+    setParam(\"STEREO_RE_CAM1\", 0.0f);\n+    setParam(\"STEREO_OF_CAM1\", 0.0f);\n+\n+    setParam(\"STEREO_MP_01\", 0.0f);\n+    setParam(\"STEREO_BAYER_D\", 0.0f);\n+    setParam(\"IMU_ENABLE\", static_cast<float>(n_cameras_));\n+    setParam(\"ADIS_IMU\", 0.0f);\n+\n+    setParam(\"STEREO_P1_CAM3\", 10.0f);\n+    setParam(\"STEREO_P2_CAM3\", 250.0f);\n+    setParam(\"STEREO_LR_CAM3\", 3.0f);\n+    setParam(\"STEREO_TH_CAM3\", 140.0f);\n+    setParam(\"STEREO_FP_CAM3\", 0.0f);\n+    setParam(\"STEREO_CE_CAM3\", 0.0f);\n+    setParam(\"STEREO_RE_CAM3\", 0.0f);\n+    setParam(\"STEREO_OF_CAM3\", 0.0f);\n+\n+    setParam(\"STEREO_P1_CAM5\", 16.0f);\n+    setParam(\"STEREO_P2_CAM5\", 240.0f);\n+    setParam(\"STEREO_LR_CAM5\", 4.0f);\n+    setParam(\"STEREO_TH_CAM5\", 120.0f);\n+    setParam(\"STEREO_FP_CAM5\", 0.0f);\n+    setParam(\"STEREO_CE_CAM5\", 0.0f);\n+    setParam(\"STEREO_RE_CAM5\", 0.0f);\n+    setParam(\"STEREO_OF_CAM5\", 0.0f);\n+\n+    setParam(\"STEREO_P1_CAM7\", 16.0f);\n+    setParam(\"STEREO_P2_CAM7\", 240.0f);\n+    setParam(\"STEREO_LR_CAM7\", 4.0f);\n+    setParam(\"STEREO_TH_CAM7\", 120.0f);\n+    setParam(\"STEREO_FP_CAM7\", 0.0f);\n+    setParam(\"STEREO_CE_CAM7\", 0.0f);\n+    setParam(\"STEREO_RE_CAM7\", 0.0f);\n+    setParam(\"STEREO_OF_CAM7\", 0.0f);\n+\n+    setParam(\"STEREO_P1_CAM9\", 16.0f);\n+    setParam(\"STEREO_P2_CAM9\", 240.0f);\n+    setParam(\"STEREO_LR_CAM9\", 4.0f);\n+    setParam(\"STEREO_TH_CAM9\", 120.0f);\n+    setParam(\"STEREO_FP_CAM9\", 0.0f);\n+    setParam(\"STEREO_CE_CAM9\", 0.0f);\n+    setParam(\"STEREO_RE_CAM9\", 0.0f);\n+    setParam(\"STEREO_OF_CAM9\", 0.0f);\n+\n+    setParam(\"CALIB_GAIN\", 4300.0f);\n+\n+    setParam(\"CAMERA_H_FLIP\", static_cast<float>(flip_));\n+    setParam(\"P_MODE\", static_cast<float>(primary_camera_mode_));\n+\n+    if (flip_) {\n+      setParam(\"IM_H_FLIP_CAM0\", 0.0f);\n+      setParam(\"IM_V_FLIP_CAM0\", 0.0f);\n+      setParam(\"IM_H_FLIP_CAM2\", 0.0f);\n+      setParam(\"IM_V_FLIP_CAM2\", 0.0f);\n+      setParam(\"IM_H_FLIP_CAM4\", 0.0f);\n+      setParam(\"IM_V_FLIP_CAM4\", 0.0f);\n+      setParam(\"IM_H_FLIP_CAM6\", 0.0f);\n+      setParam(\"IM_V_FLIP_CAM6\", 0.0f);\n+      setParam(\"IM_H_FLIP_CAM8\", 0.0f);\n+      setParam(\"IM_V_FLIP_CAM8\", 0.0f);\n+      setParam(\"IM_H_FLIP_CAM1\", 1.0f);\n+      setParam(\"IM_V_FLIP_CAM1\", 1.0f);\n+      setParam(\"IM_H_FLIP_CAM3\", 1.0f);\n+      setParam(\"IM_V_FLIP_CAM3\", 1.0f);\n+      setParam(\"IM_H_FLIP_CAM5\", 1.0f);\n+      setParam(\"IM_V_FLIP_CAM5\", 1.0f);\n+      setParam(\"IM_H_FLIP_CAM7\", 1.0f);\n+      setParam(\"IM_V_FLIP_CAM7\", 1.0f);\n+      setParam(\"IM_H_FLIP_CAM9\", 1.0f);\n+      setParam(\"IM_V_FLIP_CAM9\", 1.0f);\n+    } else {\n+      setParam(\"IM_H_FLIP_CAM0\", 1.0f);\n+      setParam(\"IM_V_FLIP_CAM0\", 1.0f);\n+      setParam(\"IM_H_FLIP_CAM2\", 1.0f);\n+      setParam(\"IM_V_FLIP_CAM2\", 1.0f);\n+      setParam(\"IM_H_FLIP_CAM4\", 1.0f);\n+      setParam(\"IM_V_FLIP_CAM4\", 1.0f);\n+      setParam(\"IM_H_FLIP_CAM6\", 1.0f);\n+      setParam(\"IM_V_FLIP_CAM6\", 1.0f);\n+      setParam(\"IM_H_FLIP_CAM8\", 1.0f);\n+      setParam(\"IM_V_FLIP_CAM8\", 1.0f);\n+      setParam(\"IM_H_FLIP_CAM1\", 0.0f);\n+      setParam(\"IM_V_FLIP_CAM1\", 0.0f);\n+      setParam(\"IM_H_FLIP_CAM3\", 0.0f);\n+      setParam(\"IM_V_FLIP_CAM3\", 0.0f);\n+      setParam(\"IM_H_FLIP_CAM5\", 0.0f);\n+      setParam(\"IM_V_FLIP_CAM5\", 0.0f);\n+      setParam(\"IM_H_FLIP_CAM7\", 0.0f);\n+      setParam(\"IM_V_FLIP_CAM7\", 0.0f);\n+      setParam(\"IM_H_FLIP_CAM9\", 0.0f);\n+      setParam(\"IM_V_FLIP_CAM9\", 0.0f);\n+    }\n+  }\n+\n+  setParam(\"SETCALIB\", float(set_calibration_));\n+\n+  setParam(\"RESETMT9V034\", 1.0f);\n+  setParam(\"RESETICM20608\", 1.0f);\n+}\n \n+void uvcROSDriver::dynamicReconfigureCallback(\n+    uvc_ros_driver::UvcDriverConfig &config, uint32_t level) {\n+  if (!shutdown_) {\n+    setParam(\"CAMERA_AUTOEXP\", static_cast<float>(config.CAMERA_AUTOEXP));\n+    setParam(\"CAMERA_EXP\", static_cast<float>(config.CAMERA_EXP));\n+    setParam(\"CAMERA_MIN_E\", static_cast<float>(config.CAMERA_MIN_E));\n+    setParam(\"CAMERA_MAX_E\", static_cast<float>(config.CAMERA_MAX_E));\n+    setParam(\"CAMERA_AUTOG\", static_cast<float>(config.CAMERA_AUTOG));\n+    setParam(\"CAMERA_GAIN\", static_cast<float>(config.CAMERA_GAIN));\n+    setParam(\"P_MODE\", static_cast<float>(config.PRIMARY_CAM_MODE));\n+\n+    setParam(\"IM_H_FLIP_CAM0\", static_cast<float>(config.CAMERA_0_HFLIP));\n+    setParam(\"IM_V_FLIP_CAM0\", static_cast<float>(config.CAMERA_0_VFLIP));\n+\n+    setParam(\"ADIS_IMU\", static_cast<float>(config.ADIS_IMU));\n+    // update camera parameters in FPGA\n+    setParam(\"UPDATEMT9V034\", 1.0f);\n+\n+    raw_enabled_ = config.RAW_ENABLED;\n+    setParam(\"CAMERA_ENABLE\",static_cast<float>(buildCameraConfig()));\n+\n+    setParam(\"STEREO_RE_CAM1\", static_cast<float>(config.STEREO_RE_CAM1));\n+    setParam(\"STEREO_CE_CAM1\", static_cast<float>(config.STEREO_CE_CAM1));\n+    setParam(\"STEREO_TH_CAM1\", static_cast<float>(config.STEREO_TH_CAM1));\n+    setParam(\"STEREO_LR_CAM1\", static_cast<float>(config.STEREO_LR_CAM1));\n+    setParam(\"STEREO_OF_CAM1\", static_cast<float>(config.STEREO_OF_CAM1));\n+    setParam(\"STEREO_P1_CAM1\", static_cast<float>(config.STEREO_P1_CAM1));\n+    setParam(\"STEREO_P2_CAM1\", static_cast<float>(config.STEREO_P2_CAM1));\n+    setParam(\"STEREO_BAYER_D\", static_cast<float>(config.STEREO_BAYER_D));\n+    // update stereo parameters in FPGA\n+    setParam(\"SETCALIB\", 1.0f);\n+  }\n }\n \n ////////////////////////////////////////////////////////////////////////////////\n // NOTE: return error really necessary?\n-uvc_error_t uvcROSDriver::initAndOpenUvc()\n-{\n-\tuvc_error_t res;\n-\t/* Initialize a UVC service context. Libuvc will set up its own libusb\n-\t* context. Replace NULL with a libusb_context pointer to run libuvc\n-\t* from an existing libusb context. */\n-\tres = uvc_init(&ctx_, NULL);\n-\n-\tif (res < 0) {\n-\t\tuvc_perror(res, \"uvc_init\");\n-\t\tROS_ERROR(\"Unable to initialize uvc service context\");\n-\t\treturn res;\n-\t}\n-\n-\t/* Locates the first attached UVC device, stores in dev */\n-\t/* filter devices: vendor_id, product_id, \"serial_num\" */\n-\tres = uvc_find_device(ctx_, &dev_, 0x04b4, 0, NULL);\n-\n-\tif (res < 0) {\n-\t\tuvc_perror(res, \"uvc_find_device\"); /* no devices found */\n-\t\tROS_ERROR(\"No devices found\");\n-\t\treturn res;\n-\t}\n-\n-\tROS_INFO(\"Device found\");\n-\n-\t/* Try to open the device: requires exclusive access */\n-\tres = uvc_open(dev_, &devh_);\n-\n-\tif (res < 0) {\n-\t\tuvc_perror(res, \"uvc_open\"); /* unable to open device */\n-\t\tROS_ERROR(\"Unable to open the device\");\n-\t\treturn res;\n-\t}\n-\n-\t// ?????????\n-\t// uvc_device_descriptor_t *desc;\n-\t// uvc_get_device_descriptor(dev_, &desc);\n-\t// uvc_free_device_descriptor(desc);\n-\n-\t/* Try to negotiate a 640x480 30 fps YUYV stream profile */\n-\tres = uvc_get_stream_ctrl_format_size(\n-\t\t      devh_, &ctrl_,              /* result stored in ctrl */\n-\t\t      UVC_FRAME_FORMAT_YUYV,      /* YUV 422, aka YUV 4:2:2. try _COMPRESSED */\n-\t\t      raw_width_, raw_height_, 30 /* width, height, fps */\n-\t      );\n-\n-\tif (res < 0) {\n-\t\t/* device doesn't provide a matching stream */\n-\t\tuvc_perror(res, \"get_mode\");\n-\t\tROS_ERROR(\"Device doesn't provide a matching stream\");\n-\t\treturn res;\n-\t}\n-\n-\treturn res;\n+uvc_error_t uvcROSDriver::initAndOpenUvc() {\n+  uvc_error_t res;\n+  /* Initialize a UVC service context. Libuvc will set up its own libusb\n+  * context. Replace NULL with a libusb_context pointer to run libuvc\n+  * from an existing libusb context. */\n+  res = uvc_init(&ctx_, NULL);\n+\n+  if (res < 0) {\n+    uvc_perror(res, \"uvc_init\");\n+    ROS_ERROR(\"Unable to initialize uvc service context\");\n+    return res;\n+  }\n+\n+  /* Locates the first attached UVC device, stores in dev */\n+  /* filter devices: vendor_id, product_id, \"serial_num\" */\n+  res = uvc_find_device(ctx_, &dev_, 0x04b4, 0, NULL);\n+\n+  if (res < 0) {\n+    uvc_perror(res, \"uvc_find_device\"); /* no devices found */\n+    ROS_ERROR(\"No devices found\");\n+    return res;\n+  }\n+\n+  ROS_INFO(\"Device found\");\n+\n+  /* Try to open the device: requires exclusive access */\n+  res = uvc_open(dev_, &devh_);\n+\n+  if (res < 0) {\n+    uvc_perror(res, \"uvc_open\"); /* unable to open device */\n+    ROS_ERROR(\"Unable to open the device\");\n+    return res;\n+  }\n+\n+  /* Try to negotiate a 640x480 30 fps YUYV stream profile */\n+  res = uvc_get_stream_ctrl_format_size(\n+      devh_, &ctrl_,              /* result stored in ctrl */\n+      UVC_FRAME_FORMAT_YUYV,      /* YUV 422, aka YUV 4:2:2. try _COMPRESSED */\n+      raw_width_, raw_height_, 30 /* width, height, fps */\n+      );\n+\n+  if (res < 0) {\n+    /* device doesn't provide a matching stream */\n+    uvc_perror(res, \"get_mode\");\n+    ROS_ERROR(\"Device doesn't provide a matching stream\");\n+    return res;\n+  }\n+\n+  return res;\n }\n \n-////////////////////////////////////////////////////////////////////////////////\n+uint8_t *uvcROSDriver::rawDataPtr(const uvc_frame_t *frame, const size_t line,\n+                                  const size_t offset) {\n+  return &(static_cast<uint8_t *>(\n+      frame->data)[2 * ((line + 1) * frame->width - offset)]);\n+}\n \n-int16_t uvcROSDriver::ShortSwap(int16_t s)\n-{\n-\tunsigned char b1, b2;\n-\tb1 = s & 255;\n-\tb2 = (s >> 8) & 255;\n-\treturn (b1 << 8) + b2;\n+uint8_t uvcROSDriver::readUInt8(const uvc_frame_t *frame, const size_t line,\n+                                const size_t offset) {\n+  return *rawDataPtr(frame, line, offset);\n }\n \n-////////////////////////////////////////////////////////////////////////////////\n+int16_t uvcROSDriver::readInt16(const uvc_frame_t *frame, const size_t line,\n+                                const size_t offset) {\n+  const uint8_t *data = rawDataPtr(frame, line, offset);\n+  uint16_t value = static_cast<uint16_t>((data[1] << 0) | (data[0] << 8));\n+  return *reinterpret_cast<int16_t *>(&value);\n+}\n+\n+uint32_t uvcROSDriver::readUInt32(const uvc_frame_t *frame, const size_t line,\n+                                  const size_t offset) {\n+  const uint8_t *data = rawDataPtr(frame, line, offset);\n \n-inline void uvcROSDriver::selectCameraInfo(int camera,\n-\t\tsensor_msgs::CameraInfo **ci)\n-{\n-\tswitch (camera) {\n-\tcase 0:\n-\t\t*ci = &info_cam_0_;\n-\t\tbreak;\n-\n-\tcase 1:\n-\t\t*ci = &info_cam_1_;\n-\t\tbreak;\n-\n-\tcase 2:\n-\t\t*ci = &info_cam_2_;\n-\t\tbreak;\n-\n-\tcase 3:\n-\t\t*ci = &info_cam_3_;\n-\t\tbreak;\n-\n-\tcase 4:\n-\t\t*ci = &info_cam_4_;\n-\t\tbreak;\n-\n-\tcase 5:\n-\t\t*ci = &info_cam_5_;\n-\t\tbreak;\n-\n-\tcase 6:\n-\t\t*ci = &info_cam_6_;\n-\t\tbreak;\n-\n-\tcase 7:\n-\t\t*ci = &info_cam_7_;\n-\t\tbreak;\n-\n-\tcase 8:\n-\t\t*ci = &info_cam_8_;\n-\t\tbreak;\n-\n-\tcase 9:\n-\t\t*ci = &info_cam_9_;\n-\t\tbreak;\n-\t}\n+  const uint32_t value =\n+      (data[3] << 0) | (data[2] << 8) | (data[1] << 16) | (data[0] << 24);\n+  return value;\n }\n \n-////////////////////////////////////////////////////////////////////////////////\n+bool uvcROSDriver::extractAndTranslateTimestamp(size_t line,\n+                                                bool update_translator,\n+                                                uvc_frame_t *frame,\n+                                                ros::Time *stamp) {\n+  // read out micro second timestamp\n+  constexpr size_t kTimestampOffset = 10;\n+  const uint32_t fpga_timestamp = readUInt32(frame, line, kTimestampOffset);\n+\n+  if (fpga_timestamp == 0) {\n+    return false;\n+  }\n+\n+  constexpr uint64_t kMicroSecondsToNanoSeconds = 1e3;\n+  const uint64_t fpga_timestamp_64 =\n+      static_cast<uint64_t>(fpga_timestamp) * kMicroSecondsToNanoSeconds;\n+\n+  // only update on image timestamps\n+  if (update_translator) {\n+    device_time_translator_->update(fpga_timestamp_64, ros::Time::now());\n+  }\n+\n+  if (device_time_translator_->isReadyToTranslate()) {\n+    *stamp = device_time_translator_->translate(fpga_timestamp);\n+  } else {\n+    return false;\n+  }\n+\n+  return true;\n+}\n+\n+CamID uvcROSDriver::extractCamId(uvc_frame_t *frame) {\n+  constexpr uint8_t kMaxCams = 10;\n+  constexpr size_t kCamIdOffset = 8;\n+  constexpr size_t kCamIdLine = 0;\n+  constexpr size_t kCamIdShift = 4;\n+  constexpr size_t kImagesPerFrame = 2;\n+  constexpr size_t kRectOffset = 8;\n+\n+  const uint8_t raw_id =\n+      readUInt8(frame, kCamIdLine, kCamIdOffset) >> kCamIdShift;\n+\n+  CamID cam_id;\n+  if (raw_id < kMaxCams / 2) {\n+    cam_id.left_cam_num = kImagesPerFrame * raw_id;\n+    cam_id.right_cam_num = cam_id.left_cam_num + 1;\n+    cam_id.is_raw_images = true;\n+  } else {\n+    cam_id.left_cam_num = kImagesPerFrame * (raw_id - kRectOffset);\n+    cam_id.right_cam_num = cam_id.left_cam_num;\n+    cam_id.is_raw_images = false;\n+  }\n+\n+  return cam_id;\n+}\n+\n+uint8_t uvcROSDriver::extractImuId(uvc_frame_t *frame) {\n+  constexpr size_t kImuIdOffset = 8;\n+  constexpr size_t kImuIdLine = 0;\n+  constexpr uint8_t kImuIdMask = 0x0F;\n+\n+  return readUInt8(frame, kImuIdLine, kImuIdOffset) & kImuIdMask;\n+}\n+\n+uint8_t uvcROSDriver::extractImuCount(size_t line, uvc_frame_t *frame) {\n+  constexpr size_t kImuCountOffset = 9;\n+  return readUInt8(frame, line, kImuCountOffset);\n+}\n+\n+bool uvcROSDriver::extractImuData(size_t line, uvc_frame_t *frame,\n+                                  sensor_msgs::Imu *msg) {\n+  if (!extractAndTranslateTimestamp(line, false, frame, &msg->header.stamp)) {\n+    return false;\n+  }\n+\n+  msg->linear_acceleration.x =\n+      extractImuElementData(line, ImuElement::AX, frame);\n+  msg->linear_acceleration.y =\n+      extractImuElementData(line, ImuElement::AY, frame);\n+  msg->linear_acceleration.z =\n+      extractImuElementData(line, ImuElement::AZ, frame);\n+\n+  msg->angular_velocity.x = extractImuElementData(line, ImuElement::RX, frame);\n+  msg->angular_velocity.y = extractImuElementData(line, ImuElement::RY, frame);\n+  msg->angular_velocity.z = extractImuElementData(line, ImuElement::RZ, frame);\n+\n+  return true;\n+}\n+\n+double uvcROSDriver::extractImuElementData(size_t line, ImuElement element,\n+                                           uvc_frame_t *frame) {\n+\n+  constexpr double kDeg2Rad = 2 * M_PI / 360.0;","path":"src/uvc_ros_driver.cpp","position":1461,"original_position":1461,"commit_id":"69d071061b8d10a542f234547f8307b1ed7f927e","original_commit_id":"69d071061b8d10a542f234547f8307b1ed7f927e","user":{"login":"fabianbl","id":14781925,"avatar_url":"https://avatars2.githubusercontent.com/u/14781925?v=4","gravatar_id":"","url":"https://api.github.com/users/fabianbl","html_url":"https://github.com/fabianbl","followers_url":"https://api.github.com/users/fabianbl/followers","following_url":"https://api.github.com/users/fabianbl/following{/other_user}","gists_url":"https://api.github.com/users/fabianbl/gists{/gist_id}","starred_url":"https://api.github.com/users/fabianbl/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/fabianbl/subscriptions","organizations_url":"https://api.github.com/users/fabianbl/orgs","repos_url":"https://api.github.com/users/fabianbl/repos","events_url":"https://api.github.com/users/fabianbl/events{/privacy}","received_events_url":"https://api.github.com/users/fabianbl/received_events","type":"User","site_admin":false},"body":"M_PI / 180.0","created_at":"2018-03-09T16:56:10Z","updated_at":"2018-03-09T17:00:32Z","html_url":"https://github.com/ethz-asl/uvc_ros_driver/pull/1#discussion_r173506890","pull_request_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/1","author_association":"NONE","_links":{"self":{"href":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/comments/173506890"},"html":{"href":"https://github.com/ethz-asl/uvc_ros_driver/pull/1#discussion_r173506890"},"pull_request":{"href":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/1"}}},"pull_request":{"url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/1","id":173795638,"html_url":"https://github.com/ethz-asl/uvc_ros_driver/pull/1","diff_url":"https://github.com/ethz-asl/uvc_ros_driver/pull/1.diff","patch_url":"https://github.com/ethz-asl/uvc_ros_driver/pull/1.patch","issue_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/issues/1","number":1,"state":"open","locked":false,"title":"Feature/refactor","user":{"login":"ZacharyTaylor","id":730680,"avatar_url":"https://avatars0.githubusercontent.com/u/730680?v=4","gravatar_id":"","url":"https://api.github.com/users/ZacharyTaylor","html_url":"https://github.com/ZacharyTaylor","followers_url":"https://api.github.com/users/ZacharyTaylor/followers","following_url":"https://api.github.com/users/ZacharyTaylor/following{/other_user}","gists_url":"https://api.github.com/users/ZacharyTaylor/gists{/gist_id}","starred_url":"https://api.github.com/users/ZacharyTaylor/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ZacharyTaylor/subscriptions","organizations_url":"https://api.github.com/users/ZacharyTaylor/orgs","repos_url":"https://api.github.com/users/ZacharyTaylor/repos","events_url":"https://api.github.com/users/ZacharyTaylor/events{/privacy}","received_events_url":"https://api.github.com/users/ZacharyTaylor/received_events","type":"User","site_admin":false},"body":"Large number of changes\r\n\r\nShould have pretty much the same functionality as the current driver but be a bit easier to read. I merged in the dynamic_rqt branch but all my changes are mainly in uvc_ros_driver.cpp\r\n\r\n-Switch to catkin_simple and yaml_cpp_catkin for easier building\r\n-Switch to cuckoo time translator for more robust timestamping\r\n-Switch to image_transport for images\r\n-Replace all switch statements with indexing\r\n-Removed a large amount of code repetition\r\n-Replace implementation specific neon instructions with valarrays\r\n-Made dedicated functions to replace some of the low level byte operations","created_at":"2018-03-08T17:12:15Z","updated_at":"2018-03-09T17:00:33Z","closed_at":null,"merged_at":null,"merge_commit_sha":"0376e5cc29bb6f0776ce67f321699c6617db5283","assignee":null,"assignees":[],"requested_reviewers":[],"requested_teams":[],"labels":[],"milestone":null,"commits_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/1/commits","review_comments_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/1/comments","review_comment_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/comments{/number}","comments_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/issues/1/comments","statuses_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/statuses/69d071061b8d10a542f234547f8307b1ed7f927e","head":{"label":"ethz-asl:feature/refactor","ref":"feature/refactor","sha":"69d071061b8d10a542f234547f8307b1ed7f927e","user":{"login":"ethz-asl","id":475362,"avatar_url":"https://avatars0.githubusercontent.com/u/475362?v=4","gravatar_id":"","url":"https://api.github.com/users/ethz-asl","html_url":"https://github.com/ethz-asl","followers_url":"https://api.github.com/users/ethz-asl/followers","following_url":"https://api.github.com/users/ethz-asl/following{/other_user}","gists_url":"https://api.github.com/users/ethz-asl/gists{/gist_id}","starred_url":"https://api.github.com/users/ethz-asl/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ethz-asl/subscriptions","organizations_url":"https://api.github.com/users/ethz-asl/orgs","repos_url":"https://api.github.com/users/ethz-asl/repos","events_url":"https://api.github.com/users/ethz-asl/events{/privacy}","received_events_url":"https://api.github.com/users/ethz-asl/received_events","type":"Organization","site_admin":false},"repo":{"id":124098241,"name":"uvc_ros_driver","full_name":"ethz-asl/uvc_ros_driver","owner":{"login":"ethz-asl","id":475362,"avatar_url":"https://avatars0.githubusercontent.com/u/475362?v=4","gravatar_id":"","url":"https://api.github.com/users/ethz-asl","html_url":"https://github.com/ethz-asl","followers_url":"https://api.github.com/users/ethz-asl/followers","following_url":"https://api.github.com/users/ethz-asl/following{/other_user}","gists_url":"https://api.github.com/users/ethz-asl/gists{/gist_id}","starred_url":"https://api.github.com/users/ethz-asl/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ethz-asl/subscriptions","organizations_url":"https://api.github.com/users/ethz-asl/orgs","repos_url":"https://api.github.com/users/ethz-asl/repos","events_url":"https://api.github.com/users/ethz-asl/events{/privacy}","received_events_url":"https://api.github.com/users/ethz-asl/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/ethz-asl/uvc_ros_driver","description":"A ros node to stream images from a multi-camera UVC device","fork":true,"url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver","forks_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/forks","keys_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/keys{/key_id}","collaborators_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/teams","hooks_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/hooks","issue_events_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/issues/events{/number}","events_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/events","assignees_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/assignees{/user}","branches_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/branches{/branch}","tags_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/tags","blobs_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/git/refs{/sha}","trees_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/git/trees{/sha}","statuses_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/statuses/{sha}","languages_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/languages","stargazers_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/stargazers","contributors_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/contributors","subscribers_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/subscribers","subscription_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/subscription","commits_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/commits{/sha}","git_commits_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/git/commits{/sha}","comments_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/comments{/number}","issue_comment_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/issues/comments{/number}","contents_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/contents/{+path}","compare_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/compare/{base}...{head}","merges_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/merges","archive_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/downloads","issues_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/issues{/number}","pulls_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls{/number}","milestones_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/milestones{/number}","notifications_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/labels{/name}","releases_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/releases{/id}","deployments_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/deployments","created_at":"2018-03-06T15:26:59Z","updated_at":"2018-03-08T17:16:22Z","pushed_at":"2018-03-09T13:35:12Z","git_url":"git://github.com/ethz-asl/uvc_ros_driver.git","ssh_url":"git@github.com:ethz-asl/uvc_ros_driver.git","clone_url":"https://github.com/ethz-asl/uvc_ros_driver.git","svn_url":"https://github.com/ethz-asl/uvc_ros_driver","homepage":null,"size":309,"stargazers_count":0,"watchers_count":0,"language":"C++","has_issues":true,"has_projects":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":0,"mirror_url":null,"archived":false,"open_issues_count":1,"license":null,"forks":0,"open_issues":1,"watchers":0,"default_branch":"master"}},"base":{"label":"ethz-asl:master","ref":"master","sha":"fac700930f18a9eb498571ec0c51a4fb3655cb63","user":{"login":"ethz-asl","id":475362,"avatar_url":"https://avatars0.githubusercontent.com/u/475362?v=4","gravatar_id":"","url":"https://api.github.com/users/ethz-asl","html_url":"https://github.com/ethz-asl","followers_url":"https://api.github.com/users/ethz-asl/followers","following_url":"https://api.github.com/users/ethz-asl/following{/other_user}","gists_url":"https://api.github.com/users/ethz-asl/gists{/gist_id}","starred_url":"https://api.github.com/users/ethz-asl/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ethz-asl/subscriptions","organizations_url":"https://api.github.com/users/ethz-asl/orgs","repos_url":"https://api.github.com/users/ethz-asl/repos","events_url":"https://api.github.com/users/ethz-asl/events{/privacy}","received_events_url":"https://api.github.com/users/ethz-asl/received_events","type":"Organization","site_admin":false},"repo":{"id":124098241,"name":"uvc_ros_driver","full_name":"ethz-asl/uvc_ros_driver","owner":{"login":"ethz-asl","id":475362,"avatar_url":"https://avatars0.githubusercontent.com/u/475362?v=4","gravatar_id":"","url":"https://api.github.com/users/ethz-asl","html_url":"https://github.com/ethz-asl","followers_url":"https://api.github.com/users/ethz-asl/followers","following_url":"https://api.github.com/users/ethz-asl/following{/other_user}","gists_url":"https://api.github.com/users/ethz-asl/gists{/gist_id}","starred_url":"https://api.github.com/users/ethz-asl/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ethz-asl/subscriptions","organizations_url":"https://api.github.com/users/ethz-asl/orgs","repos_url":"https://api.github.com/users/ethz-asl/repos","events_url":"https://api.github.com/users/ethz-asl/events{/privacy}","received_events_url":"https://api.github.com/users/ethz-asl/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/ethz-asl/uvc_ros_driver","description":"A ros node to stream images from a multi-camera UVC device","fork":true,"url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver","forks_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/forks","keys_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/keys{/key_id}","collaborators_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/teams","hooks_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/hooks","issue_events_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/issues/events{/number}","events_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/events","assignees_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/assignees{/user}","branches_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/branches{/branch}","tags_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/tags","blobs_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/git/refs{/sha}","trees_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/git/trees{/sha}","statuses_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/statuses/{sha}","languages_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/languages","stargazers_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/stargazers","contributors_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/contributors","subscribers_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/subscribers","subscription_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/subscription","commits_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/commits{/sha}","git_commits_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/git/commits{/sha}","comments_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/comments{/number}","issue_comment_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/issues/comments{/number}","contents_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/contents/{+path}","compare_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/compare/{base}...{head}","merges_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/merges","archive_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/downloads","issues_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/issues{/number}","pulls_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls{/number}","milestones_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/milestones{/number}","notifications_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/labels{/name}","releases_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/releases{/id}","deployments_url":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/deployments","created_at":"2018-03-06T15:26:59Z","updated_at":"2018-03-08T17:16:22Z","pushed_at":"2018-03-09T13:35:12Z","git_url":"git://github.com/ethz-asl/uvc_ros_driver.git","ssh_url":"git@github.com:ethz-asl/uvc_ros_driver.git","clone_url":"https://github.com/ethz-asl/uvc_ros_driver.git","svn_url":"https://github.com/ethz-asl/uvc_ros_driver","homepage":null,"size":309,"stargazers_count":0,"watchers_count":0,"language":"C++","has_issues":true,"has_projects":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":0,"mirror_url":null,"archived":false,"open_issues_count":1,"license":null,"forks":0,"open_issues":1,"watchers":0,"default_branch":"master"}},"_links":{"self":{"href":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/1"},"html":{"href":"https://github.com/ethz-asl/uvc_ros_driver/pull/1"},"issue":{"href":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/issues/1"},"comments":{"href":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/issues/1/comments"},"review_comments":{"href":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/1/comments"},"review_comment":{"href":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/comments{/number}"},"commits":{"href":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/pulls/1/commits"},"statuses":{"href":"https://api.github.com/repos/ethz-asl/uvc_ros_driver/statuses/69d071061b8d10a542f234547f8307b1ed7f927e"}},"author_association":"NONE"}}
{"actor":{"display_login":"fabianbl"}}