SE-2310

CMakeLists.txt

@@ -40,7 +40,9 @@ find_package(camera_calibration_parsers REQUIRED)
 find_package(OpenCV 4 REQUIRED)
 
 add_library(bottlenose_camera_driver SHARED
-        src/bottlenose_camera_driver.cpp)
+        src/bottlenose_camera_driver.cpp
+        src/calibration_parser.cpp
+        )
 target_compile_definitions(bottlenose_camera_driver
   PRIVATE "COMPOSITION_BUILDING_DLL")
 target_include_directories(bottlenose_camera_driver

config/camera.yaml

@@ -1,6 +1,6 @@
-image_width: 640
-image_height: 480
-camera_name: camera
+image_width: 1920
+image_height: 1440
+camera_name: Bottlenose
 camera_matrix:
   rows: 3
   cols: 3

include/bottlenose_camera_driver.hpp

@@ -85,8 +85,16 @@ namespace bottlenose_camera_driver {
     std::thread m_management_thread;      ///< Management thread handle.
 
     std::shared_ptr<camera_info_manager::CameraInfoManager> m_cinfo_manager;
-    /// Camera publisher.
-    image_transport::CameraPublisher m_camera_pub;
+    /// Camera publisher: color image (unrectified).
+    image_transport::CameraPublisher m_image_color;
+    /// Camera publisher: color image (rectified).
+    image_transport::CameraPublisher m_image_rect_color;
+    /// Camera publisher: color image, sensor 1 (unrectified).
+    image_transport::CameraPublisher m_image_color_1;
+    /// Camera publisher: color image, sensor 1 (rectified).
+    image_transport::CameraPublisher m_image_rect_color_1;
+    /// Camera publisher: depth stream.
+    image_transport::CameraPublisher m_depth_image_rect;
 };
 } // namespace bottlenose_camera_driver
 #endif //__BOTTLENOSE_CAMERA_DRIVER_HPP__

include/bottlenose_parameters.hpp

@@ -27,11 +27,19 @@ typedef struct {
   const rclcpp::ParameterValue & default_value;
 } parameter_t;
 
+typedef enum {
+  MODE_MONO,
+  MODE_STEREO,
+  MODE_DEPTH,
+  /* MODE_RGBD: FIXME: as soon as the FW update rolls in for this */
+} camera_mode_t;
+
 const parameter_t bottlenose_parameters[] = {
   {"frame_id", rclcpp::ParameterValue("camera")},
-  {"format", rclcpp::ParameterValue("1920x1440")},
+  {"camera_calibration_file", rclcpp::ParameterValue("package://bottlenose_camera_driver/config/camera.yaml")},
   {"mac_address", rclcpp::ParameterValue("00:00:00:00:00:00")},
   {"keep_partial", rclcpp::ParameterValue(false)},
+  {"mode", rclcpp::ParameterValue(0)},
   /* Device parameters */
   {"fps", rclcpp::ParameterValue(20.0)},
   {"exposure", rclcpp::ParameterValue(20.0)},

include/calibration_parser.hpp

@@ -0,0 +1,19 @@
+/******************************************************************************
+ *  Copyright 2023 Labforge Inc.                                              *
+ *                                                                            *
+ * Licensed under the Apache License, Version 2.0 (the "License");            *
+ * you may not use this project except in compliance with the License.        *
+ * You may obtain a copy of the License at                                    *
+ *                                                                            *
+ *     http://www.apache.org/licenses/LICENSE-2.0                             *
+ *                                                                            *
+ * Unless required by applicable law or agreed to in writing, software        *
+ * distributed under the License is distributed on an "AS IS" BASIS,          *
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
+ * See the License for the specific language governing permissions and        *
+ * limitations under the License.                                             *
+ ******************************************************************************
+
+@file calibration_parser.hpp Calibration Parser for Bottlenose
+@author Thomas Reidemeister <thomas@labforge.ca>
+*/

src/bottlenose_camera_driver.cpp

@@ -67,14 +67,14 @@ CameraDriver::CameraDriver(const rclcpp::NodeOptions &node_options) : Node("bott
   }
 
   rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
-  m_camera_pub = image_transport::create_camera_publisher(this, "image_raw", custom_qos_profile);
+  m_image_color = image_transport::create_camera_publisher(this, "image_color", custom_qos_profile);
+  m_image_rect_color = image_transport::create_camera_publisher(this, "m_image_rect_color", custom_qos_profile);
+  m_image_color_1 = image_transport::create_camera_publisher(this, "image_color_1", custom_qos_profile);
+  m_image_rect_color_1 = image_transport::create_camera_publisher(this, "m_image_rect_color_1", custom_qos_profile);
+  m_depth_image_rect = image_transport::create_camera_publisher(this, "depth/image_rect_color", custom_qos_profile);
 
   m_cinfo_manager = std::make_shared<camera_info_manager::CameraInfoManager>(this);
 
-//  /* get ROS2 config parameter for camera calibration file */
-//  auto camera_calibration_file_param_ = this->declare_parameter("camera_calibration_file", "file://config/camera.yaml");
-//  m_cinfo_manager->loadCameraInfo(camera_calibration_file_param_);
-
   m_timer = this->create_wall_timer(1ms, std::bind(&CameraDriver::status_callback, this));
 }
 
@@ -105,11 +105,16 @@ void CameraDriver::status_callback() {
       m_management_thread.join();
     }
   }
-  auto mac_address = this->get_parameter("mac_address").as_string();
+  auto mac_address = get_parameter("mac_address").as_string();
   if(mac_address == "00:00:00:00:00:00") {
     RCLCPP_INFO_ONCE(get_logger(), "Bottlenose undefined please set mac_address");
     return;
   }
+  string calibration_file = get_parameter("camera_calibration_file").as_string();
+  if(!m_cinfo_manager->loadCameraInfo(calibration_file)) {
+    RCLCPP_ERROR(get_logger(), "Failed to load camera calibration file %s", calibration_file.c_str());
+    return;
+  }
   m_mac_address = mac_address;
   done = false;
   m_management_thread = std::thread(&CameraDriver::management_thread, this);
@@ -149,7 +154,7 @@ std::shared_ptr<sensor_msgs::msg::Image> CameraDriver::convertFrameToMessage(PvB
     uint64_t nanoseconds = (image->GetTimestamp() - seconds * 1e6) * 1e3; //nanoseconds
     ros_image.header.stamp.nanosec = nanoseconds;
     ros_image.header.stamp.sec = seconds;
-    ros_image.header.frame_id = this->get_parameter("frame_id").as_string();
+    ros_image.header.frame_id = get_parameter("frame_id").as_string();
 
     auto msg_ptr_ = std::make_shared<sensor_msgs::msg::Image>(ros_image);
     return msg_ptr_;
@@ -283,23 +288,10 @@ bool CameraDriver::set_format() {
     RCLCPP_ERROR(get_logger(), "Could not configure format");
     return false;
   }
-  // Find the position of 'x' in the format specification string
-  auto format = get_parameter("format").as_string();
-  auto xPos = format.find('x');
-
-  // Extract the width and height substrings
-  std::string widthStr = format.substr(0, xPos);
-  std::string heightStr = format.substr(xPos + 1);
-
-  // Convert the width and height strings to integers
-  int width = std::stoi(widthStr);
-  int height = std::stoi(heightStr);
-
-  // Print the width and height
-  RCLCPP_DEBUG_STREAM(get_logger(), "Decoded format " << width << " x " << height);
-  PvResult res = heightParam->SetValue(height);
+  m_cinfo_manager->getCameraInfo().height;
+  PvResult res = heightParam->SetValue(m_cinfo_manager->getCameraInfo().height);
   if(res.IsFailure()) {
-    RCLCPP_ERROR_STREAM(get_logger(), "Could not configure format to " << format);
+    RCLCPP_ERROR(get_logger(), "Could not configure format, check your camera configuration");
     return false;
   }
   // Wait for parameter pass-through
@@ -308,14 +300,17 @@ bool CameraDriver::set_format() {
   int64_t width_in;
   res = widthParam->GetValue(width_in);
   if(res.IsFailure()) {
-    RCLCPP_ERROR_STREAM(get_logger(), "Could not configure format to " << format);
+    RCLCPP_ERROR_STREAM(get_logger(), "Could not configure format to " <<
+      m_cinfo_manager->getCameraInfo().width << " x " << m_cinfo_manager->getCameraInfo().height);
     return false;
   }
-  if(width_in != width) {
-    RCLCPP_ERROR_STREAM(get_logger(), "Could not configure format to " << format << " actual format is " << width_in << " x " << height);
+  if(width_in != m_cinfo_manager->getCameraInfo().width) {
+    RCLCPP_ERROR_STREAM(get_logger(), "Could not configure format to " <<
+      m_cinfo_manager->getCameraInfo().width << " x " << m_cinfo_manager->getCameraInfo().height);
     return false;
   }
-  RCLCPP_DEBUG_STREAM(get_logger(), "Configured format to " << format);
+  RCLCPP_DEBUG_STREAM(get_logger(), "Configured format to " <<
+    m_cinfo_manager->getCameraInfo().width << " x " << m_cinfo_manager->getCameraInfo().height);
 
   return true;
 }
@@ -496,7 +491,7 @@ void CameraDriver::management_thread() {
               sensor_msgs::msg::CameraInfo::SharedPtr info_msg(
                   new sensor_msgs::msg::CameraInfo(m_cinfo_manager->getCameraInfo()));
               info_msg->header = m_image_msg->header;
-              m_camera_pub.publish(m_image_msg, info_msg);
+              m_image_color.publish(m_image_msg, info_msg);
             }
             break;
 

src/calibration_parser.cpp

@@ -0,0 +1,102 @@
+/******************************************************************************
+ *  Copyright 2023 Labforge Inc.                                              *
+ *                                                                            *
+ * Licensed under the Apache License, Version 2.0 (the "License");            *
+ * you may not use this project except in compliance with the License.        *
+ * You may obtain a copy of the License at                                    *
+ *                                                                            *
+ *     http://www.apache.org/licenses/LICENSE-2.0                             *
+ *                                                                            *
+ * Unless required by applicable law or agreed to in writing, software        *
+ * distributed under the License is distributed on an "AS IS" BASIS,          *
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
+ * See the License for the specific language governing permissions and        *
+ * limitations under the License.                                             *
+ ******************************************************************************
+
+@file calibration_parser.cpp Implementation of Calibration Parser for Bottlenose
+@author Thomas Reidemeister <thomas@labforge.ca>
+*/
+#include <iostream>
+#include <fstream>
+#include <unordered_map>
+#include <yaml-cpp/yaml.h>
+
+std::unordered_map<std::string, double> LoadFlatYamlParameters(const std::string &fname = "", int sensors = 0) {
+  std::unordered_map<std::string, double> kdata;
+
+  if (fname.empty() || (fname.length() >= 5 &&
+                        (fname.substr(fname.length() - 5) == ".yaml" || fname.substr(fname.length() - 4) == ".yml"))) {
+    return kdata;
+  }
+
+  if (sensors == 0) {
+    return kdata;
+  }
+
+  try {
+    YAML::Node calib = YAML::LoadFile(fname);
+
+    int nCameras = calib.size();
+    if (nCameras != sensors) {
+      return kdata;
+    }
+
+    int tvec_count = 0;
+    int rvec_count = 0;
+
+    for (const auto &cam: calib) {
+      std::string camKey = cam.first.as<std::string>();
+
+      if (camKey != "cam0" && camKey != "cam1") {
+        kdata.clear();
+        return kdata;
+      }
+
+      std::string id = camKey.substr(camKey.length() - 1);
+
+      kdata["fx" + id] = cam.second["fx"].as<double>();
+      kdata["fy" + id] = cam.second["fy"].as<double>();
+      kdata["cx" + id] = cam.second["cx"].as<double>();
+      kdata["cy" + id] = cam.second["cy"].as<double>();
+
+      kdata["k1" + id] = cam.second["k1"].as<double>();
+      kdata["k2" + id] = cam.second["k2"] ? cam.second["k2"].as<double>() : 0.0;
+      kdata["k3" + id] = cam.second["k3"] ? cam.second["k3"].as<double>() : 0.0;
+      kdata["p1" + id] = cam.second["p1"] ? cam.second["p1"].as<double>() : 0.0;
+      kdata["p2" + id] = cam.second["p2"] ? cam.second["p2"].as<double>() : 0.0;
+
+      std::vector<double> tvec = {0.0, 0.0, 0.0};
+      if (cam.second["tvec"]) {
+        tvec = cam.second["tvec"].as<std::vector<double>>();
+        tvec_count++;
+      }
+
+      std::vector<double> rvec = {0.0, 0.0, 0.0};
+      if (cam.second["rvec"]) {
+        rvec = cam.second["rvec"].as<std::vector<double>>();
+        rvec_count++;
+      }
+
+      kdata["tx" + id] = tvec[0];
+      kdata["ty" + id] = tvec[1];
+      kdata["tz" + id] = tvec[2];
+      kdata["rx" + id] = rvec[0];
+      kdata["ry" + id] = rvec[1];
+      kdata["rz" + id] = rvec[2];
+
+      kdata["kWidth"] = cam.second["width"].as<double>();
+      kdata["kHeight"] = cam.second["height"].as<double>();
+    }
+
+    if (tvec_count < (sensors - 1) || rvec_count < (sensors - 1)) {
+      kdata.clear();
+      return kdata;
+    }
+  } catch (const std::exception &e) {
+    kdata.clear();
+    return kdata;
+  }
+
+  return kdata;
+}