From 85e86048d4f1016aa7eea177103c759a296c0585 Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Thu, 8 Jun 2023 17:58:53 -0400 Subject: [PATCH 1/4] First stab at calibration parser --- CMakeLists.txt | 4 +- include/calibration_parser.hpp | 19 ++++++ src/calibration_parser.cpp | 102 +++++++++++++++++++++++++++++++++ 3 files changed, 124 insertions(+), 1 deletion(-) create mode 100644 include/calibration_parser.hpp create mode 100644 src/calibration_parser.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 71610f9..62eced7 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/calibration_parser.hpp b/include/calibration_parser.hpp new file mode 100644 index 0000000..b12b309 --- /dev/null +++ b/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 +*/ \ No newline at end of file diff --git a/src/calibration_parser.cpp b/src/calibration_parser.cpp new file mode 100644 index 0000000..f5e162c --- /dev/null +++ b/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 +*/ +#include +#include +#include +#include + +std::unordered_map LoadFlatYamlParameters(const std::string &fname = "", int sensors = 0) { + std::unordered_map 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(); + + if (camKey != "cam0" && camKey != "cam1") { + kdata.clear(); + return kdata; + } + + std::string id = camKey.substr(camKey.length() - 1); + + kdata["fx" + id] = cam.second["fx"].as(); + kdata["fy" + id] = cam.second["fy"].as(); + kdata["cx" + id] = cam.second["cx"].as(); + kdata["cy" + id] = cam.second["cy"].as(); + + kdata["k1" + id] = cam.second["k1"].as(); + kdata["k2" + id] = cam.second["k2"] ? cam.second["k2"].as() : 0.0; + kdata["k3" + id] = cam.second["k3"] ? cam.second["k3"].as() : 0.0; + kdata["p1" + id] = cam.second["p1"] ? cam.second["p1"].as() : 0.0; + kdata["p2" + id] = cam.second["p2"] ? cam.second["p2"].as() : 0.0; + + std::vector tvec = {0.0, 0.0, 0.0}; + if (cam.second["tvec"]) { + tvec = cam.second["tvec"].as>(); + tvec_count++; + } + + std::vector rvec = {0.0, 0.0, 0.0}; + if (cam.second["rvec"]) { + rvec = cam.second["rvec"].as>(); + 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(); + kdata["kHeight"] = cam.second["height"].as(); + } + + if (tvec_count < (sensors - 1) || rvec_count < (sensors - 1)) { + kdata.clear(); + return kdata; + } + } catch (const std::exception &e) { + kdata.clear(); + return kdata; + } + + return kdata; +} From 3653f46830340d7d47cf0da29b13bf414375dd17 Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Fri, 9 Jun 2023 13:04:15 -0400 Subject: [PATCH 2/4] First stab at interface definition --- README.md | 27 ++++++++++++++++++++++++++- src/bottlenose_camera_driver.cpp | 2 +- 2 files changed, 27 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 4387b4a..73b1802 100644 --- a/README.md +++ b/README.md @@ -97,10 +97,35 @@ exceeded the camera will drop frames. ``` bottlenose_camera_driver | - +-- camera_image_raw : Color image stream of Bottlenose camera + +-- camera_info : Camera calibration data + +-- image_color : Color image stream of Bottlenose camera (unrectified, if rectification is disabled) + +-- image_color_1 : Color image stream of Bottlenose camera (unrectified, if rectification is disabled), + | stereo only + +-- image_rect_color : Rectified color image stream of Bottlenose camera (if rectification is enabled) + +-- image_rect_color_1 : Rectified color image stream of Bottlenose camera (if rectification is enabled), + | stereo only + +-- depth + +-- image_rect_color : Rectified disparity image stream of Bottlenose camera (if depth is enabled) ``` +In the current release the topics ```image_color```, ```image_rect_color``` and ```depth/image_rect_color``` +are exclusive. See the below for the available configurations: + + * ```mode=0``` ***Color streaming*** + * ```rectify=0``` -> ```image_color{_1}``` (*default*) + * ```rectify=1``` -> ```image_rect_color{_1}``` + * ***Stereo only*** + * Set sensor to ```0``` or ```1``` to switch between left or right sensor + + * ```mode=1``` ***Stereo streaming*** + * ```rectify=0``` -> ```image_color``` and ```image_color_1``` + * ```rectify=1``` -> ```image_rect_color``` and ```image_rect_color_1``` + + * ```mode=3``` ***Disparity streaming*** + * ```rectify=1``` -> ```depth/image_rect_color``` + * Note ```rectify=0``` is not supported for depth streaming ## References * Bottlenose [Getting Started](https://docs.labforge.ca/docs/getting-started) + * Bottlenose [Calibration Guide](https://docs.labforge.ca/docs/3d-modules) * Bottlenose [SDK Demos](https://github.com/labforge/sdk-demos) diff --git a/src/bottlenose_camera_driver.cpp b/src/bottlenose_camera_driver.cpp index dd0a1aa..7f73807 100644 --- a/src/bottlenose_camera_driver.cpp +++ b/src/bottlenose_camera_driver.cpp @@ -67,7 +67,7 @@ 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_camera_pub = image_transport::create_camera_publisher(this, "image_color", custom_qos_profile); m_cinfo_manager = std::make_shared(this); From 243fac1cefec392069e1e83adaf6dbfc449e7bcf Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Fri, 9 Jun 2023 15:00:36 -0400 Subject: [PATCH 3/4] Added topics, parsing format now out of camera calibration --- README.md | 105 ++++++++++++++------------- config/camera.yaml | 6 +- include/bottlenose_camera_driver.hpp | 12 ++- include/bottlenose_parameters.hpp | 2 +- src/bottlenose_camera_driver.cpp | 47 ++++++------ 5 files changed, 89 insertions(+), 83 deletions(-) diff --git a/README.md b/README.md index 73b1802..3b7e606 100644 --- a/README.md +++ b/README.md @@ -35,57 +35,60 @@ ros2 run image_view image_view --ros-args --remap /image:=/image_raw ### Available parameters -| Parameter | Description | Default | Run-time adjustable | -|-----------------------------------------|-------------------------------------------------------------------------------|-------------------------|---------------------| -| ```mac_address``` | The MAC address of Bottlenose | ```00:00:00:00:00:00``` | :x: | -| ```frame_id``` | The frame_id embedded in image headers | ```camera``` | :heavy_check_mark: | -| ```keep_partial``` | Keep partial images (i.e. corrupted in transmission) | ```false``` | :heavy_check_mark: | -| ```format``` | Format of the camera (*) | ```1920x1440``` | :x: | -| ```fps``` | Target frames per second (*) | ```20``` | :x: | -| ***Image Sensor(s) Controls*** | (**) | | | -| ```exposure``` | Exposure time in milliseconds | ```20``` | :heavy_check_mark: | -| ```gain``` | Analog gain | ```1.0``` | :heavy_check_mark: | -| ```dGainBlue``` | Digital gain for blue pixels in Bayer array | ```1024``` | :heavy_check_mark: | -| ```dgainGB``` | Digital gain for green-blue pixels in Bayer array | ```1024``` | :heavy_check_mark: | -| ```dgainGR``` | Digital gain for green-red pixels in Bayer array | ```1024``` | :heavy_check_mark: | -| ```dGainRed``` | Digital gain for red pixels in Bayer array | ```1024``` | :heavy_check_mark: | -| **Image Processing Controls** | | | | -| ```gamma``` | Gamma correction of the image | ```2.2``` | :heavy_check_mark: | -| ```blackBlue``` | Black level for blue pixels in Bayer array | ```255``` | :heavy_check_mark: | -| ```blackGB``` | Black level for green-blue pixels in Bayer array | ```255``` | :heavy_check_mark: | -| ```blackGR``` | Black level for green-red pixels in Bayer array | ```255``` | :heavy_check_mark: | -| ```blackRed``` | Black level for red pixels in Bayer array | ```255``` | :heavy_check_mark: | -| ```blackGainBlue``` | Black gain for blue pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | -| ```blackGainGB``` | Black gain for green-blue pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | -| ```blackGainGR``` | Black gain for green-red pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | -| ```blackGainRed``` | Black gain for red pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | -| ```brightness``` | Brightness of the image | ```-13107``` | :heavy_check_mark: | -| ```linearContrast``` | Linear contrast of the image | ```136``` | :heavy_check_mark: | -| ```wbBlue``` | White balance for blue component | ```1.0``` | :heavy_check_mark: | -| ```wbGreen``` | White balance for green component | ```1.0``` | :heavy_check_mark: | -| ```wbRed``` | White balance for red component | ```1.0``` | :heavy_check_mark: | -| **Color Correction Controls** | | | | -| ```custom_ccm``` | Enable custom color correction matrix | ```false``` | :x: | -| ```CCMValue00``` | Color correction matrix value at row 0, column 0 (only if custom_ccm=true) | ```1.0``` | :x: | -| ```CCMValue01``` | Color correction matrix value at row 0, column 1 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue02``` | Color correction matrix value at row 0, column 2 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue10``` | Color correction matrix value at row 1, column 0 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue11``` | Color correction matrix value at row 1, column 1 (only if custom_ccm=true) | ```1.0``` | :x: | -| ```CCMValue12``` | Color correction matrix value at row 1, column 2 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue20``` | Color correction matrix value at row 2, column 0 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue21``` | Color correction matrix value at row 2, column 1 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue22``` | Color correction matrix value at row 2, column 2 (only if custom_ccm=true) | ```1.0``` | :x: | -| **GigE Vision Stream Parameters** | | | | -| ```AnswerTimeout``` | Time the GigE Vision Device can take for command response. | ```100``` | :x: | -| ```CommandRetryCount``` | Command attempts before it is considered as failed | ```50``` | :x: | -| ```MaximumPendingResends``` | Maximum number of packets in a block that can be missing | ```0``` | :x: | -| ```MaximumResendRequestRetryByPacket``` | The maximum number of times a resend request can be issued. | ```0``` | :x: | -| ```MaximumResendGroupSize``` | Maximum number of packets to resend at once | ```0``` | :x: | -| ```ResendRequestTimeout``` | Timeout for resend requests in (us) | ```100``` | :x: | -| ```RequestTimeout``` | Maximum time that the data receiver waits for all the packets of a block (ms) | ```10000``` | :x: | -| ```ResetOnIdle``` | Time without packets before resetting itself | ```2000``` | :x: | - -(*) Note: effective limitations are imposed by available bandwidth for the chosen configuration. If the bandwidth is +| Parameter | Description | Default | Run-time adjustable | +|-----------------------------------------|-------------------------------------------------------------------------------|---------------------------|---------------------| +| ```mac_address``` | The MAC address of Bottlenose | ```00:00:00:00:00:00``` | :x: | +| ```frame_id``` | The frame_id embedded in image headers | ```camera``` | :heavy_check_mark: | +| ```keep_partial``` | Keep partial images (i.e. corrupted in transmission) | ```false``` | :heavy_check_mark: | +| ```camera_calibration_file``` | Camera Calibration File (*) | ```config/camera.yaml``` | :x: | +| ```fps``` | Target frames per second (**) | ```20``` | :x: | +| ***Image Sensor(s) Controls*** | (***) | | | +| ```exposure``` | Exposure time in milliseconds | ```20``` | :heavy_check_mark: | +| ```gain``` | Analog gain | ```1.0``` | :heavy_check_mark: | +| ```dGainBlue``` | Digital gain for blue pixels in Bayer array | ```1024``` | :heavy_check_mark: | +| ```dgainGB``` | Digital gain for green-blue pixels in Bayer array | ```1024``` | :heavy_check_mark: | +| ```dgainGR``` | Digital gain for green-red pixels in Bayer array | ```1024``` | :heavy_check_mark: | +| ```dGainRed``` | Digital gain for red pixels in Bayer array | ```1024``` | :heavy_check_mark: | +| **Image Processing Controls** | | | | +| ```gamma``` | Gamma correction of the image | ```2.2``` | :heavy_check_mark: | +| ```blackBlue``` | Black level for blue pixels in Bayer array | ```255``` | :heavy_check_mark: | +| ```blackGB``` | Black level for green-blue pixels in Bayer array | ```255``` | :heavy_check_mark: | +| ```blackGR``` | Black level for green-red pixels in Bayer array | ```255``` | :heavy_check_mark: | +| ```blackRed``` | Black level for red pixels in Bayer array | ```255``` | :heavy_check_mark: | +| ```blackGainBlue``` | Black gain for blue pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | +| ```blackGainGB``` | Black gain for green-blue pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | +| ```blackGainGR``` | Black gain for green-red pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | +| ```blackGainRed``` | Black gain for red pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | +| ```brightness``` | Brightness of the image | ```-13107``` | :heavy_check_mark: | +| ```linearContrast``` | Linear contrast of the image | ```136``` | :heavy_check_mark: | +| ```wbBlue``` | White balance for blue component | ```1.0``` | :heavy_check_mark: | +| ```wbGreen``` | White balance for green component | ```1.0``` | :heavy_check_mark: | +| ```wbRed``` | White balance for red component | ```1.0``` | :heavy_check_mark: | +| **Color Correction Controls** | | | | +| ```custom_ccm``` | Enable custom color correction matrix | ```false``` | :x: | +| ```CCMValue00``` | Color correction matrix value at row 0, column 0 (only if custom_ccm=true) | ```1.0``` | :x: | +| ```CCMValue01``` | Color correction matrix value at row 0, column 1 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue02``` | Color correction matrix value at row 0, column 2 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue10``` | Color correction matrix value at row 1, column 0 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue11``` | Color correction matrix value at row 1, column 1 (only if custom_ccm=true) | ```1.0``` | :x: | +| ```CCMValue12``` | Color correction matrix value at row 1, column 2 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue20``` | Color correction matrix value at row 2, column 0 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue21``` | Color correction matrix value at row 2, column 1 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue22``` | Color correction matrix value at row 2, column 2 (only if custom_ccm=true) | ```1.0``` | :x: | +| **GigE Vision Stream Parameters** | | | | +| ```AnswerTimeout``` | Time the GigE Vision Device can take for command response. | ```100``` | :x: | +| ```CommandRetryCount``` | Command attempts before it is considered as failed | ```50``` | :x: | +| ```MaximumPendingResends``` | Maximum number of packets in a block that can be missing | ```0``` | :x: | +| ```MaximumResendRequestRetryByPacket``` | The maximum number of times a resend request can be issued. | ```0``` | :x: | +| ```MaximumResendGroupSize``` | Maximum number of packets to resend at once | ```0``` | :x: | +| ```ResendRequestTimeout``` | Timeout for resend requests in (us) | ```100``` | :x: | +| ```RequestTimeout``` | Maximum time that the data receiver waits for all the packets of a block (ms) | ```10000``` | :x: | +| ```ResetOnIdle``` | Time without packets before resetting itself | ```2000``` | :x: | + +(*) Note: Required parameter. Even if the image stream is desired to be raw, the calibration dictates the resolution of +the image. + +(**) Note: effective limitations are imposed by available bandwidth for the chosen configuration. If the bandwidth is exceeded the camera will drop frames. (**) Note: For stereo cameras the controls are applied to both sensors simultaneously. diff --git a/config/camera.yaml b/config/camera.yaml index eebff7e..eb0bfda 100755 --- a/config/camera.yaml +++ b/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 diff --git a/include/bottlenose_camera_driver.hpp b/include/bottlenose_camera_driver.hpp index 9717809..ea078d4 100644 --- a/include/bottlenose_camera_driver.hpp +++ b/include/bottlenose_camera_driver.hpp @@ -85,8 +85,16 @@ namespace bottlenose_camera_driver { std::thread m_management_thread; ///< Management thread handle. std::shared_ptr 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__ diff --git a/include/bottlenose_parameters.hpp b/include/bottlenose_parameters.hpp index db36451..2672956 100644 --- a/include/bottlenose_parameters.hpp +++ b/include/bottlenose_parameters.hpp @@ -29,7 +29,7 @@ typedef struct { const parameter_t bottlenose_parameters[] = { {"frame_id", rclcpp::ParameterValue("camera")}, - {"format", rclcpp::ParameterValue("1920x1440")}, + {"camera_calibration_file", rclcpp::ParameterValue("file://config/camera.yaml")}, {"mac_address", rclcpp::ParameterValue("00:00:00:00:00:00")}, {"keep_partial", rclcpp::ParameterValue(false)}, /* Device parameters */ diff --git a/src/bottlenose_camera_driver.cpp b/src/bottlenose_camera_driver.cpp index 7f73807..157433a 100644 --- a/src/bottlenose_camera_driver.cpp +++ b/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_color", 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(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)); } @@ -110,6 +110,11 @@ void CameraDriver::status_callback() { RCLCPP_INFO_ONCE(get_logger(), "Bottlenose undefined please set mac_address"); return; } + string calibration_file = this->get_parameter("camera_calibration_file").as_string(); + if(!m_cinfo_manager->loadCameraInfo(get_parameter(calibration_file).as_string())) { + 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); @@ -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; From 5335d407f4cd8d9fbfed919658e8bc0aec99a4ff Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Fri, 9 Jun 2023 15:30:33 -0400 Subject: [PATCH 4/4] Using ROS2 config file to parse format --- README.md | 1 + include/bottlenose_parameters.hpp | 10 +++++++++- src/bottlenose_camera_driver.cpp | 8 ++++---- 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 3b7e606..449fef3 100644 --- a/README.md +++ b/README.md @@ -41,6 +41,7 @@ ros2 run image_view image_view --ros-args --remap /image:=/image_raw | ```frame_id``` | The frame_id embedded in image headers | ```camera``` | :heavy_check_mark: | | ```keep_partial``` | Keep partial images (i.e. corrupted in transmission) | ```false``` | :heavy_check_mark: | | ```camera_calibration_file``` | Camera Calibration File (*) | ```config/camera.yaml``` | :x: | +| ```mode``` | Camera output (**) | ```0``` | :x: | | ```fps``` | Target frames per second (**) | ```20``` | :x: | | ***Image Sensor(s) Controls*** | (***) | | | | ```exposure``` | Exposure time in milliseconds | ```20``` | :heavy_check_mark: | diff --git a/include/bottlenose_parameters.hpp b/include/bottlenose_parameters.hpp index 2672956..f521526 100644 --- a/include/bottlenose_parameters.hpp +++ b/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")}, - {"camera_calibration_file", rclcpp::ParameterValue("file://config/camera.yaml")}, + {"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)}, diff --git a/src/bottlenose_camera_driver.cpp b/src/bottlenose_camera_driver.cpp index 157433a..cfaa129 100644 --- a/src/bottlenose_camera_driver.cpp +++ b/src/bottlenose_camera_driver.cpp @@ -105,13 +105,13 @@ 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 = this->get_parameter("camera_calibration_file").as_string(); - if(!m_cinfo_manager->loadCameraInfo(get_parameter(calibration_file).as_string())) { + 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; } @@ -154,7 +154,7 @@ std::shared_ptr 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(ros_image); return msg_ptr_;