From 4534c95d963add749730eb8b4fee8db6ad74291b Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sat, 17 Feb 2024 23:04:05 -0500 Subject: [PATCH 1/7] Initial Merging: Setting up ImageSaverNode Signed-off-by: CursedRock17 --- image_view/doc/components.rst | 6 +++--- image_view/include/image_view/image_saver_node.hpp | 1 + image_view/src/image_saver_node.cpp | 4 +++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/image_view/doc/components.rst b/image_view/doc/components.rst index ef7f26e1e..abbdf4c6a 100644 --- a/image_view/doc/components.rst +++ b/image_view/doc/components.rst @@ -31,7 +31,7 @@ This tool allows you to save images as jpg/png file from streaming can run with: .. code-block:: bash - + ros2 run image_view image_saver --ros-args -r image:=[your topic] or see this answer to control the timing of capture. @@ -74,7 +74,7 @@ This tool allows you to save images as jpg/png file from streaming can run with: .. code-block:: bash - + ros2 run image_view image_saver --ros-args -r image:=[your topic] or see this answer to control the timing of capture. @@ -93,7 +93,7 @@ Services Parameters ---------- * **encoding** (string, default:"bgr8"): Encoding type of input image topic. - * **filename_format** (string, default: "left%04i.jpg"): File name for + * **filename_format** (string, default: "frame%04i.jpg"): File name for saved images, you can use '%04i' for sequence number, and '%s' for default file format, you can use 'jpg' ,'png', 'pgm' as filename suffixes. * **image_transport** (string, default: raw): Image transport to use. diff --git a/image_view/include/image_view/image_saver_node.hpp b/image_view/include/image_view/image_saver_node.hpp index 5d8c76b2b..3a4d56ec0 100644 --- a/image_view/include/image_view/image_saver_node.hpp +++ b/image_view/include/image_view/image_saver_node.hpp @@ -68,6 +68,7 @@ class ImageSaverNode private: std::string g_format; + double fps_; bool stamped_filename_; bool save_all_image_{false}; bool save_image_service_{false}; diff --git a/image_view/src/image_saver_node.cpp b/image_view/src/image_saver_node.cpp index 075d84853..563ff98df 100644 --- a/image_view/src/image_saver_node.cpp +++ b/image_view/src/image_saver_node.cpp @@ -93,7 +93,8 @@ ImageSaverNode::ImageSaverNode(const rclcpp::NodeOptions & options) &ImageSaverNode::callbackWithoutCameraInfo, this, std::placeholders::_1), hints.getTransport()); - g_format = this->declare_parameter("filename_format", std::string("left%04i.%s")); + g_format = this->declare_parameter("filename_format", std::string("frame%04i.%s")); + fps_ = this->declare_parameter("fps", 0.1); encoding_ = this->declare_parameter("encoding", std::string("bgr8")); save_all_image_ = this->declare_parameter("save_all_image", true); stamped_filename_ = this->declare_parameter("stamped_filename", false); @@ -125,6 +126,7 @@ bool ImageSaverNode::saveImage( { cv::Mat image; try { + encoding_ = image_msg->encoding; image = cv_bridge::toCvShare(image_msg, encoding_)->image; } catch (const cv_bridge::Exception &) { RCLCPP_ERROR( From b18c008de832441390b135369fa96d9371fe9786 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sun, 18 Feb 2024 11:55:31 -0500 Subject: [PATCH 2/7] Removing ExtractImagesNode Signed-off-by: CursedRock17 --- image_view/CMakeLists.txt | 12 -- image_view/doc/components.rst | 30 +--- .../image_view/extract_images_node.hpp | 85 ---------- image_view/src/extract_images.cpp | 67 -------- image_view/src/extract_images_node.cpp | 145 ------------------ image_view/src/image_saver_node.cpp | 4 +- 6 files changed, 3 insertions(+), 340 deletions(-) delete mode 100644 image_view/include/image_view/extract_images_node.hpp delete mode 100644 image_view/src/extract_images.cpp delete mode 100644 image_view/src/extract_images_node.cpp diff --git a/image_view/CMakeLists.txt b/image_view/CMakeLists.txt index 1148b0669..82e59b587 100644 --- a/image_view/CMakeLists.txt +++ b/image_view/CMakeLists.txt @@ -21,7 +21,6 @@ endif() ament_auto_add_library(${PROJECT_NAME}_nodes SHARED src/disparity_view_node.cpp - src/extract_images_node.cpp src/image_view_node.cpp src/image_saver_node.cpp src/stereo_view_node.cpp @@ -35,7 +34,6 @@ target_compile_definitions(${PROJECT_NAME}_nodes ) rclcpp_components_register_nodes(${PROJECT_NAME}_nodes "${PROJECT_NAME}::DisparityViewNode" - "${PROJECT_NAME}::ExtractImagesNode" "${PROJECT_NAME}::ImageViewNode" "${PROJECT_NAME}::ImageSaverNode" "${PROJECT_NAME}::StereoViewNode" @@ -74,16 +72,6 @@ add_dependencies(stereo_view ) # Other Tools -ament_auto_add_executable(extract_images - src/extract_images.cpp -) -target_link_libraries(extract_images - ${PROJECT_NAME}_nodes -) -add_dependencies(extract_images - ${PROJECT_NAME}_nodes -) - ament_auto_add_executable(image_saver src/image_saver.cpp ) diff --git a/image_view/doc/components.rst b/image_view/doc/components.rst index abbdf4c6a..ca4e0b54e 100644 --- a/image_view/doc/components.rst +++ b/image_view/doc/components.rst @@ -18,35 +18,6 @@ Parameters * **window_name** (string, default: name of the image topic): The name of the display window. -image_view::ExtractImagesNode ------------------------------ -This tool also allows you to save images as jpg/png file from -streaming (ROS sensor_msgs/Image topic) to a file. -``image_saver`` node provide very similar functionalities, -such as providing service call to trigger the node to save -images, save images other than JPEG format, etc. - -This tool allows you to save images as jpg/png file from streaming -(ROS sensor_msgs/Image topic) to a file. From command line, you -can run with: - -.. code-block:: bash - - ros2 run image_view image_saver --ros-args -r image:=[your topic] - -or see this answer to control the timing of capture. - -Subscribed Topics -^^^^^^^^^^^^^^^^^ - * **image** (sensor_msgs/Image): Image topic to visualize. - -Parameters ----------- - * **filename_format** (string, default: "frame%04i.jpg"): File name for - saved images, you must add use '%04i' for sequence number. - * **image_transport** (string, default: raw): Image transport to use. - * **sec_per_frame** (double, default: 0.1): Seconds between saved frames. - image_view::ImageViewNode ------------------------- Simple image viewer for ROS sensor_msgs/Image topics. Node name @@ -93,6 +64,7 @@ Services Parameters ---------- * **encoding** (string, default:"bgr8"): Encoding type of input image topic. + * **fps** (double, default:"0.0"): Seconds between saved frames. * **filename_format** (string, default: "frame%04i.jpg"): File name for saved images, you can use '%04i' for sequence number, and '%s' for default file format, you can use 'jpg' ,'png', 'pgm' as filename suffixes. diff --git a/image_view/include/image_view/extract_images_node.hpp b/image_view/include/image_view/extract_images_node.hpp deleted file mode 100644 index 37d07ffbe..000000000 --- a/image_view/include/image_view/extract_images_node.hpp +++ /dev/null @@ -1,85 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -// Copyright 2019, Joshua Whitley -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file 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. - -#ifndef IMAGE_VIEW__EXTRACT_IMAGES_NODE_HPP_ -#define IMAGE_VIEW__EXTRACT_IMAGES_NODE_HPP_ - -#include -#include - -#include -#include -#include - -namespace image_view -{ - -class ExtractImagesNode - : public rclcpp::Node -{ -public: - explicit ExtractImagesNode(const rclcpp::NodeOptions & options); - -private: - image_transport::Subscriber sub_; - - sensor_msgs::msg::Image::ConstSharedPtr last_msg_; - std::mutex image_mutex_; - - std::string window_name_; - std::string filename_format_; - int count_; - rclcpp::Time _time; - double sec_per_frame_; - - void image_cb(const sensor_msgs::msg::Image::ConstSharedPtr & msg); -}; - -} // namespace image_view - -#endif // IMAGE_VIEW__EXTRACT_IMAGES_NODE_HPP_ diff --git a/image_view/src/extract_images.cpp b/image_view/src/extract_images.cpp deleted file mode 100644 index 7dd4bdccb..000000000 --- a/image_view/src/extract_images.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -// Copyright 2019, Joshua Whitley -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file 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. - -#include - -#include - -#include - -int main(int argc, char ** argv) -{ - using image_view::ExtractImagesNode; - - rclcpp::init(argc, argv); - - rclcpp::NodeOptions options; - auto ein = std::make_shared(options); - - rclcpp::spin(ein); - - return 0; -} diff --git a/image_view/src/extract_images_node.cpp b/image_view/src/extract_images_node.cpp deleted file mode 100644 index 271ced3a9..000000000 --- a/image_view/src/extract_images_node.cpp +++ /dev/null @@ -1,145 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -// Copyright 2019, Joshua Whitley -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file 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. - -#include -#include -#include - -#include "cv_bridge/cv_bridge.hpp" - -#include - -#include -#include -#include -#include - -#include "image_view/extract_images_node.hpp" -#include "utils.hpp" - -namespace image_view -{ - -ExtractImagesNode::ExtractImagesNode(const rclcpp::NodeOptions & options) -: rclcpp::Node("extract_images_node", options), - filename_format_(""), count_(0), _time(this->now()) -{ - // For compressed topics to remap appropriately, we need to pass a - // fully expanded and remapped topic name to image_transport - auto node_base = this->get_node_base_interface(); - std::string topic = node_base->resolve_topic_or_service_name("image", false); - - // TransportHints does not actually declare the parameter - this->declare_parameter("image_transport", "raw"); - image_transport::TransportHints hints(this); - std::string transport = this->get_parameter("transport").as_string(); - - sub_ = image_transport::create_subscription( - this, topic, std::bind( - &ExtractImagesNode::image_cb, this, std::placeholders::_1), - hints.getTransport()); - - auto topics = this->get_topic_names_and_types(); - - if (topics.find(topic) == topics.end()) { - RCLCPP_WARN( - this->get_logger(), "extract_images: image has not been remapped! " - "Typical command-line usage:\n\t$ ros2 run image_view extract_images " - "--ros-args -r image:= -p transport:="); - } - - this->declare_parameter("filename_format", std::string("frame%04i.jpg")); - filename_format_ = this->get_parameter("filename_format").as_string(); - - this->declare_parameter("sec_per_frame", 0.1); - sec_per_frame_ = this->get_parameter("sec_per_frame").as_double(); - - RCLCPP_INFO(this->get_logger(), "Initialized sec per frame to %f", sec_per_frame_); -} - -void ExtractImagesNode::image_cb(const sensor_msgs::msg::Image::ConstSharedPtr & msg) -{ - std::lock_guard guard(image_mutex_); - - // Hang on to message pointer for sake of mouse_cb - last_msg_ = msg; - - // May want to view raw bayer data - // NB: This is hacky, but should be OK since we have only one image CB. - if (msg->encoding.find("bayer") != std::string::npos) { - std::const_pointer_cast(msg)->encoding = "mono8"; - } - - cv::Mat image; - try { - image = cv_bridge::toCvShare(msg, "bgr8")->image; - } catch (const cv_bridge::Exception &) { - RCLCPP_ERROR(this->get_logger(), "Unable to convert %s image to bgr8", msg->encoding.c_str()); - } - - rclcpp::Duration delay = this->now() - _time; - - if (delay.seconds() >= sec_per_frame_) { - _time = this->now(); - - if (!image.empty()) { - std::string filename = string_format(filename_format_, count_); - - cv::imwrite(filename, image); - - RCLCPP_INFO(this->get_logger(), "Saved image %s", filename.c_str()); - count_++; - } else { - RCLCPP_WARN(this->get_logger(), "Couldn't save image, no data!"); - } - } -} - -} // namespace image_view - -RCLCPP_COMPONENTS_REGISTER_NODE(image_view::ExtractImagesNode) diff --git a/image_view/src/image_saver_node.cpp b/image_view/src/image_saver_node.cpp index 563ff98df..fe4614e1a 100644 --- a/image_view/src/image_saver_node.cpp +++ b/image_view/src/image_saver_node.cpp @@ -94,7 +94,7 @@ ImageSaverNode::ImageSaverNode(const rclcpp::NodeOptions & options) hints.getTransport()); g_format = this->declare_parameter("filename_format", std::string("frame%04i.%s")); - fps_ = this->declare_parameter("fps", 0.1); + fps_ = this->declare_parameter("fps", 0.0); encoding_ = this->declare_parameter("encoding", std::string("bgr8")); save_all_image_ = this->declare_parameter("save_all_image", true); stamped_filename_ = this->declare_parameter("stamped_filename", false); @@ -126,7 +126,7 @@ bool ImageSaverNode::saveImage( { cv::Mat image; try { - encoding_ = image_msg->encoding; + std::string encoding = image_msg->encoding.empty() ? encoding_ : image_msg->encoding; image = cv_bridge::toCvShare(image_msg, encoding_)->image; } catch (const cv_bridge::Exception &) { RCLCPP_ERROR( From c85f1aea3e2eafaa85c87a9ddc04ef71e2ab3159 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sun, 18 Feb 2024 11:56:44 -0500 Subject: [PATCH 3/7] Removing Typo Signed-off-by: CursedRock17 --- image_view/src/image_saver_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/image_view/src/image_saver_node.cpp b/image_view/src/image_saver_node.cpp index fe4614e1a..0258dd728 100644 --- a/image_view/src/image_saver_node.cpp +++ b/image_view/src/image_saver_node.cpp @@ -127,7 +127,7 @@ bool ImageSaverNode::saveImage( cv::Mat image; try { std::string encoding = image_msg->encoding.empty() ? encoding_ : image_msg->encoding; - image = cv_bridge::toCvShare(image_msg, encoding_)->image; + image = cv_bridge::toCvShare(image_msg, encoding)->image; } catch (const cv_bridge::Exception &) { RCLCPP_ERROR( this->get_logger(), "Unable to convert %s image to %s", From e21c7d604df5b3824c227daaa7601abd284cf790 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sat, 24 Feb 2024 01:30:37 -0500 Subject: [PATCH 4/7] Adding an fps feature Signed-off-by: CursedRock17 --- .../include/image_view/image_saver_node.hpp | 6 +- image_view/src/image_saver_node.cpp | 61 ++++++++++++++----- 2 files changed, 51 insertions(+), 16 deletions(-) diff --git a/image_view/include/image_view/image_saver_node.hpp b/image_view/include/image_view/image_saver_node.hpp index 3a4d56ec0..ecfd36941 100644 --- a/image_view/include/image_view/image_saver_node.hpp +++ b/image_view/include/image_view/image_saver_node.hpp @@ -68,7 +68,7 @@ class ImageSaverNode private: std::string g_format; - double fps_; + int fps_; bool stamped_filename_; bool save_all_image_{false}; bool save_image_service_{false}; @@ -77,6 +77,9 @@ class ImageSaverNode bool is_first_image_{true}; bool has_camera_info_{false}; size_t count_{0u}; + sensor_msgs::msg::Image::ConstSharedPtr latest_image_; + std::mutex image_save_mutex_; + rclcpp::TimerBase::SharedPtr timer_; rclcpp::Time start_time_; rclcpp::Time end_time_; image_transport::CameraSubscriber cam_sub_; @@ -85,6 +88,7 @@ class ImageSaverNode rclcpp::Service::SharedPtr start_srv_; rclcpp::Service::SharedPtr end_srv_; + void timerCallback(); bool saveImage(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, std::string & filename); bool service( const std::shared_ptr request_header, diff --git a/image_view/src/image_saver_node.cpp b/image_view/src/image_saver_node.cpp index 0258dd728..064d4dc42 100644 --- a/image_view/src/image_saver_node.cpp +++ b/image_view/src/image_saver_node.cpp @@ -66,6 +66,8 @@ #include "utils.hpp" +using namespace std::chrono_literals; + namespace image_view { @@ -88,13 +90,14 @@ ImageSaverNode::ImageSaverNode(const rclcpp::NodeOptions & options) hints.getTransport()); // Useful when CameraInfo is not being published + image_sub_ = image_transport::create_subscription( this, topic, std::bind( &ImageSaverNode::callbackWithoutCameraInfo, this, std::placeholders::_1), hints.getTransport()); g_format = this->declare_parameter("filename_format", std::string("frame%04i.%s")); - fps_ = this->declare_parameter("fps", 0.0); + fps_ = this->declare_parameter("fps", 0); encoding_ = this->declare_parameter("encoding", std::string("bgr8")); save_all_image_ = this->declare_parameter("save_all_image", true); stamped_filename_ = this->declare_parameter("stamped_filename", false); @@ -119,6 +122,22 @@ ImageSaverNode::ImageSaverNode(const rclcpp::NodeOptions & options) &ImageSaverNode::callbackEndSave, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } + + if(fps_ != 0) { + using fps_timer = std::chrono::duration>; + timer_ = this->create_wall_timer(fps_timer(fps_), std::bind(&ImageSaverNode::timerCallback, this)); + } +} + +void ImageSaverNode::timerCallback() +{ + // save the image + std::string filename; + if (!saveImage(latest_image_, filename)) { + return; + } + + count_++; } bool ImageSaverNode::saveImage( @@ -233,13 +252,19 @@ void ImageSaverNode::callbackWithoutCameraInfo( } } - // save the image - std::string filename; - if (!saveImage(image_msg, filename)) { - return; + if (fps_ != 0) { + std::lock_guard lock = std::lock_guard(image_save_mutex_); + latest_image_ = image_msg; } + else { + // save the image + std::string filename; + if (!saveImage(image_msg, filename)) { + return; + } - count_++; + count_++; + } } void ImageSaverNode::callbackWithCameraInfo( @@ -258,19 +283,25 @@ void ImageSaverNode::callbackWithCameraInfo( } } - // save the image - std::string filename; - if (!saveImage(image_msg, filename)) { - return; + if (fps_ != 0) { + std::lock_guard lock = std::lock_guard(image_save_mutex_); + latest_image_ = image_msg; } + else { + // save the image + std::string filename; + if (!saveImage(image_msg, filename)) { + return; + } + // save the CameraInfo + if (info) { + filename = filename.replace(filename.rfind("."), filename.length(), ".ini"); + camera_calibration_parsers::writeCalibration(filename, "camera", *info); + } - // save the CameraInfo - if (info) { - filename = filename.replace(filename.rfind("."), filename.length(), ".ini"); - camera_calibration_parsers::writeCalibration(filename, "camera", *info); + count_++; } - count_++; } } // namespace image_view From dac21ff731e8cd260fd6693ab21fc3295132e454 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sat, 24 Feb 2024 20:17:31 -0500 Subject: [PATCH 5/7] Fix docs Signed-off-by: CursedRock17 --- image_view/doc/components.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/image_view/doc/components.rst b/image_view/doc/components.rst index ca4e0b54e..04bb27d3b 100644 --- a/image_view/doc/components.rst +++ b/image_view/doc/components.rst @@ -64,7 +64,7 @@ Services Parameters ---------- * **encoding** (string, default:"bgr8"): Encoding type of input image topic. - * **fps** (double, default:"0.0"): Seconds between saved frames. + * **fps** (int, default:"0"): Seconds between saved frames. * **filename_format** (string, default: "frame%04i.jpg"): File name for saved images, you can use '%04i' for sequence number, and '%s' for default file format, you can use 'jpg' ,'png', 'pgm' as filename suffixes. From 421a706728c2fc962a4a8c8a645f3088f2079b3e Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sat, 24 Feb 2024 20:25:36 -0500 Subject: [PATCH 6/7] Fixing cpplint Signed-off-by: CursedRock17 --- image_view/src/image_saver_node.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/image_view/src/image_saver_node.cpp b/image_view/src/image_saver_node.cpp index 064d4dc42..aa0a0d62c 100644 --- a/image_view/src/image_saver_node.cpp +++ b/image_view/src/image_saver_node.cpp @@ -123,9 +123,10 @@ ImageSaverNode::ImageSaverNode(const rclcpp::NodeOptions & options) std::placeholders::_3)); } - if(fps_ != 0) { + if (fps_ != 0) { using fps_timer = std::chrono::duration>; - timer_ = this->create_wall_timer(fps_timer(fps_), std::bind(&ImageSaverNode::timerCallback, this)); + timer_ = this->create_wall_timer( + fps_timer(fps_), std::bind(&ImageSaverNode::timerCallback, this)); } } @@ -255,8 +256,7 @@ void ImageSaverNode::callbackWithoutCameraInfo( if (fps_ != 0) { std::lock_guard lock = std::lock_guard(image_save_mutex_); latest_image_ = image_msg; - } - else { + } else { // save the image std::string filename; if (!saveImage(image_msg, filename)) { @@ -286,8 +286,7 @@ void ImageSaverNode::callbackWithCameraInfo( if (fps_ != 0) { std::lock_guard lock = std::lock_guard(image_save_mutex_); latest_image_ = image_msg; - } - else { + } else { // save the image std::string filename; if (!saveImage(image_msg, filename)) { @@ -301,7 +300,6 @@ void ImageSaverNode::callbackWithCameraInfo( count_++; } - } } // namespace image_view From 7f5d1d1e09529d8e08836205f015036cf47f141f Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sat, 17 Feb 2024 23:04:05 -0500 Subject: [PATCH 7/7] Adding a parameter for camera calibration Signed-off-by: CursedRock17 Revert "Initial Merging: Setting up ImageSaverNode" This reverts commit 4534c95d963add749730eb8b4fee8db6ad74291b. DisparityNode: replace full_dp parameter with sgbm_mode (#945) Previously, only the SGBM and HH modes were allowed add invalid_depth param (#943) Add option to set all invalid depth pixels to a specified value, typically the maximum range. * Updates convertDepth parameter name and optimizes use of the parameter. * Updates PointCloudXYZ, PointCloudXYZI, and PointCloudXYZRGB with new invalid_depth parameter Adding scale parameter to camera calibrator Signed-off-by: CursedRock17 --- .gitignore | 2 +- .../src/camera_calibration/calibrator.py | 8 ++++- depth_image_proc/doc/components.rst | 6 ++++ .../include/depth_image_proc/conversions.hpp | 29 ++++++++++++------- .../depth_image_proc/point_cloud_xyz.hpp | 3 ++ .../depth_image_proc/point_cloud_xyzi.hpp | 3 ++ .../depth_image_proc/point_cloud_xyzrgb.hpp | 3 ++ depth_image_proc/src/conversions.cpp | 6 ++-- depth_image_proc/src/point_cloud_xyz.cpp | 9 ++++-- depth_image_proc/src/point_cloud_xyzi.cpp | 7 +++-- depth_image_proc/src/point_cloud_xyzrgb.cpp | 7 +++-- image_pipeline/doc/changelog.rst | 4 +++ stereo_image_proc/doc/components.rst | 8 +++++ .../launch/stereo_image_proc.launch.py | 6 ++-- .../src/stereo_image_proc/disparity_node.cpp | 18 +++++------- 15 files changed, 83 insertions(+), 36 deletions(-) diff --git a/.gitignore b/.gitignore index 9d93f2973..189be055b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,3 @@ *pyc -.vscode/settings.json +.vscode/ */doc/generated diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index a25c80021..b7440d5e2 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -343,6 +343,7 @@ def __init__(self, boards, flags=0, fisheye_flags = 0, pattern=Patterns.Chessboa self.pattern = pattern self.br = cv_bridge.CvBridge() self.camera_model = CAMERA_MODEL.PINHOLE + self.declare_parameter('vga_scale', 0) # self.db is list of (parameters, image) samples for use in calibration. parameters has form # (X, Y, size, skew) all normalized to [0,1], to keep track of what sort of samples we've taken # and ensure enough variety. @@ -535,7 +536,12 @@ def downsample_and_detect(self, img): # Scale the input image down to ~VGA size height = img.shape[0] width = img.shape[1] - scale = math.sqrt( (width*height) / (640.*480.) ) + vga_scale_param = self.get_parameter('vga_scale').get_parameter_value() + if vga_scale_param == 0: + scale = math.sqrt((width*height) / (640.*480.)) + else: + scale = vga_scale_param + if scale > 1.0: scrib = cv2.resize(img, (int(width / scale), int(height / scale))) else: diff --git a/depth_image_proc/doc/components.rst b/depth_image_proc/doc/components.rst index c0a783c60..19d870a73 100644 --- a/depth_image_proc/doc/components.rst +++ b/depth_image_proc/doc/components.rst @@ -116,6 +116,8 @@ Parameters for the depth topic subscriber. * **queue_size** (int, default: 5): Size of message queue for synchronizing subscribed topics. + * **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth + values (if 0.0 the parameter has no effect). depth_image_proc::PointCloudXyzRadialNode ----------------------------------------- @@ -165,6 +167,8 @@ Parameters the intensity image subscriber. * **queue_size** (int, default: 5): Size of message queue for synchronizing subscribed topics. + * **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth + values (if 0.0 the parameter has no effect). depth_image_proc::PointCloudXyziRadialNode ------------------------------------------ @@ -235,6 +239,8 @@ Parameters * **exact_sync** (bool, default: False): Whether to use exact synchronizer. * **queue_size** (int, default: 5): Size of message queue for synchronizing subscribed topics. + * **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth + values (if 0.0 the parameter has no effect). depth_image_proc::PointCloudXyzrgbRadialNode -------------------------------------------- diff --git a/depth_image_proc/include/depth_image_proc/conversions.hpp b/depth_image_proc/include/depth_image_proc/conversions.hpp index 11496320f..8af177bd0 100644 --- a/depth_image_proc/include/depth_image_proc/conversions.hpp +++ b/depth_image_proc/include/depth_image_proc/conversions.hpp @@ -51,9 +51,9 @@ namespace depth_image_proc template void convertDepth( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, const image_geometry::PinholeCameraModel & model, - double range_max = 0.0) + double invalid_depth = 0.0) { // Use correct principal point from calibration float center_x = model.cx(); @@ -65,19 +65,26 @@ void convertDepth( float constant_y = unit_scaling / model.fy(); float bad_point = std::numeric_limits::quiet_NaN(); + // ensure that the computation only happens in case we have a default depth + T invalid_depth_cvt = T(0); + bool use_invalid_depth = invalid_depth != 0.0; + if (use_invalid_depth) { + invalid_depth_cvt = DepthTraits::fromMeters(invalid_depth); + } sensor_msgs::PointCloud2Iterator iter_x(*cloud_msg, "x"); sensor_msgs::PointCloud2Iterator iter_y(*cloud_msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(*cloud_msg, "z"); + const T * depth_row = reinterpret_cast(&depth_msg->data[0]); - int row_step = depth_msg->step / sizeof(T); - for (int v = 0; v < static_cast(cloud_msg->height); ++v, depth_row += row_step) { - for (int u = 0; u < static_cast(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z) { + uint32_t row_step = depth_msg->step / sizeof(T); + for (uint32_t v = 0; v < cloud_msg->height; ++v, depth_row += row_step) { + for (uint32_t u = 0; u < cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z) { T depth = depth_row[u]; // Missing points denoted by NaNs if (!DepthTraits::valid(depth)) { - if (range_max != 0.0) { - depth = DepthTraits::fromMeters(range_max); + if (use_invalid_depth) { + depth = invalid_depth_cvt; } else { *iter_x = *iter_y = *iter_z = bad_point; continue; @@ -96,8 +103,8 @@ void convertDepth( template void convertDepthRadial( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, - cv::Mat & transform) + const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + const cv::Mat & transform) { // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) float bad_point = std::numeric_limits::quiet_NaN(); @@ -129,7 +136,7 @@ void convertDepthRadial( template void convertIntensity( const sensor_msgs::msg::Image::ConstSharedPtr & intensity_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg) + const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg) { sensor_msgs::PointCloud2Iterator iter_i(*cloud_msg, "intensity"); const T * inten_row = reinterpret_cast(&intensity_msg->data[0]); @@ -145,7 +152,7 @@ void convertIntensity( // Handles RGB8, BGR8, and MONO8 void convertRgb( const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg, int red_offset, int green_offset, int blue_offset, int color_step); cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial); diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp index c5fc4bfd2..6818e8cd8 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp @@ -67,6 +67,9 @@ class PointCloudXyzNode : public rclcpp::Node image_transport::CameraSubscriber sub_depth_; int queue_size_; + // Parameters + double invalid_depth_; + // Publications std::mutex connect_mutex_; rclcpp::Publisher::SharedPtr pub_point_cloud_; diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp index 1a9e754a2..cb079cc1d 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp @@ -71,6 +71,9 @@ class PointCloudXyziNode : public rclcpp::Node using Synchronizer = message_filters::Synchronizer; std::shared_ptr sync_; + // parameters + float invalid_depth_; + // Publications std::mutex connect_mutex_; rclcpp::Publisher::SharedPtr pub_point_cloud_; diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp index f2b99c165..47c7e0cba 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp @@ -75,6 +75,9 @@ class PointCloudXyzrgbNode : public rclcpp::Node std::shared_ptr sync_; std::shared_ptr exact_sync_; + // parameters + float invalid_depth_; + // Publications std::mutex connect_mutex_; rclcpp::Publisher::SharedPtr pub_point_cloud_; diff --git a/depth_image_proc/src/conversions.cpp b/depth_image_proc/src/conversions.cpp index ba462323f..697734414 100644 --- a/depth_image_proc/src/conversions.cpp +++ b/depth_image_proc/src/conversions.cpp @@ -79,7 +79,7 @@ cv::Mat initMatrix( void convertRgb( const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, int red_offset, int green_offset, int blue_offset, int color_step) { sensor_msgs::PointCloud2Iterator iter_r(*cloud_msg, "r"); @@ -101,8 +101,8 @@ void convertRgb( // force template instantiation template void convertDepth( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, const image_geometry::PinholeCameraModel & model, - double range_max); + double invalid_depth); } // namespace depth_image_proc diff --git a/depth_image_proc/src/point_cloud_xyz.cpp b/depth_image_proc/src/point_cloud_xyz.cpp index 0175d929a..227a4100b 100644 --- a/depth_image_proc/src/point_cloud_xyz.cpp +++ b/depth_image_proc/src/point_cloud_xyz.cpp @@ -58,6 +58,9 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options) // Read parameters queue_size_ = this->declare_parameter("queue_size", 5); + // values used for invalid points for pcd conversion + invalid_depth_ = this->declare_parameter("invalid_depth", 0.0); + // Create publisher with connect callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = @@ -94,7 +97,7 @@ void PointCloudXyzNode::depthCb( const Image::ConstSharedPtr & depth_msg, const CameraInfo::ConstSharedPtr & info_msg) { - auto cloud_msg = std::make_shared(); + const PointCloud2::SharedPtr cloud_msg = std::make_shared(); cloud_msg->header = depth_msg->header; cloud_msg->height = depth_msg->height; cloud_msg->width = depth_msg->width; @@ -109,9 +112,9 @@ void PointCloudXyzNode::depthCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == enc::TYPE_16UC1 || depth_msg->encoding == enc::MONO16) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else if (depth_msg->encoding == enc::TYPE_32FC1) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); diff --git a/depth_image_proc/src/point_cloud_xyzi.cpp b/depth_image_proc/src/point_cloud_xyzi.cpp index 9b373d86d..0abc0ff8c 100644 --- a/depth_image_proc/src/point_cloud_xyzi.cpp +++ b/depth_image_proc/src/point_cloud_xyzi.cpp @@ -58,6 +58,9 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options) this->declare_parameter("image_transport", "raw"); this->declare_parameter("depth_image_transport", "raw"); + // value used for invalid points for pcd conversion + invalid_depth_ = this->declare_parameter("invalid_depth", 0.0); + // Read parameters int queue_size = this->declare_parameter("queue_size", 5); @@ -202,9 +205,9 @@ void PointCloudXyziNode::imageCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == enc::TYPE_16UC1) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else if (depth_msg->encoding == enc::TYPE_32FC1) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); diff --git a/depth_image_proc/src/point_cloud_xyzrgb.cpp b/depth_image_proc/src/point_cloud_xyzrgb.cpp index 3347b11e1..5ef736d96 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb.cpp @@ -55,6 +55,9 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options) this->declare_parameter("image_transport", "raw"); this->declare_parameter("depth_image_transport", "raw"); + // value used for invalid points for pcd conversion + invalid_depth_ = this->declare_parameter("invalid_depth", 0.0); + // Read parameters int queue_size = this->declare_parameter("queue_size", 5); bool use_exact_sync = this->declare_parameter("exact_sync", false); @@ -256,9 +259,9 @@ void PointCloudXyzrgbNode::imageCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); diff --git a/image_pipeline/doc/changelog.rst b/image_pipeline/doc/changelog.rst index b69fb0887..c96ed2101 100644 --- a/image_pipeline/doc/changelog.rst +++ b/image_pipeline/doc/changelog.rst @@ -38,3 +38,7 @@ There are several major change between ``Iron`` and ``Jazzy``: ``rgb/image_rect_color`` to ``rgb/image_raw`` to make clear that the unrectified camera projection matrix is used, and for consistency with other radial nodes. + * The boolen parameter ``full_dp`` from the DisparityNode has been deleted + and a new integer parameter ``sgbm_mode`` added to enable all the + variations of the stereo matching algorithm SGBM available from the + OpenCV library. diff --git a/stereo_image_proc/doc/components.rst b/stereo_image_proc/doc/components.rst index 5ac76213e..8fe3ac66a 100644 --- a/stereo_image_proc/doc/components.rst +++ b/stereo_image_proc/doc/components.rst @@ -30,6 +30,14 @@ Published Topics Parameters ^^^^^^^^^^ +*Disparity algorithm variant* + * **sgbm_mode** (int, default: 0): Stereo matching algorithm variation: + + * SGBM (0) + * HH (1) + * SGBM_3WAY (2) + * HH4 (3) + *Disparity pre-filtering* * **prefilter_size** (int, default: 9): Normalization window size, pixels. diff --git a/stereo_image_proc/launch/stereo_image_proc.launch.py b/stereo_image_proc/launch/stereo_image_proc.launch.py index 8c2c81cba..d58343684 100644 --- a/stereo_image_proc/launch/stereo_image_proc.launch.py +++ b/stereo_image_proc/launch/stereo_image_proc.launch.py @@ -69,7 +69,7 @@ def generate_launch_description(): 'uniqueness_ratio': LaunchConfiguration('uniqueness_ratio'), 'P1': LaunchConfiguration('P1'), 'P2': LaunchConfiguration('P2'), - 'full_dp': LaunchConfiguration('full_dp'), + 'sgbm_mode': LaunchConfiguration('sgbm_mode'), }], remappings=[ ('left/image_rect', [LaunchConfiguration('left_namespace'), '/image_rect']), @@ -199,8 +199,8 @@ def generate_launch_description(): '(Semi-Global Block Matching only)' ), DeclareLaunchArgument( - name='full_dp', default_value='False', - description='Run the full variant of the algorithm (Semi-Global Block Matching only)' + name='sgbm_mode', default_value='0', + description='The mode of the SGBM matcher to be used' ), ComposableNodeContainer( condition=LaunchConfigurationEquals('container', ''), diff --git a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp index 97a32f7ad..a5f9c2d63 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -258,6 +258,12 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) "Maximum allowed difference in the left-right disparity check in pixels" " (Semi-Global Block Matching only)", 0, 0, 128, 1); + add_param_to_map( + int_params, + "sgbm_mode", + "Mode of the SGBM stereo matcher." + "", + 0, 0, 3, 1); // Describe double parameters std::map> double_params; @@ -277,17 +283,9 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) "The second parameter ccontrolling the disparity smoothness (Semi-Global Block Matching only)", 400.0, 0.0, 4000.0, 0.0); - // Describe bool parameters - std::map> bool_params; - rcl_interfaces::msg::ParameterDescriptor full_dp_descriptor; - full_dp_descriptor.description = - "Run the full variant of the algorithm (Semi-Global Block Matching only)"; - bool_params["full_dp"] = std::make_pair(false, full_dp_descriptor); - // Declaring parameters triggers the previously registered callback this->declare_parameters("", int_params); this->declare_parameters("", double_params); - this->declare_parameters("", bool_params); // Publisher options to allow reconfigurable qos settings and connect callback rclcpp::PublisherOptions pub_opts; @@ -424,8 +422,8 @@ rcl_interfaces::msg::SetParametersResult DisparityNode::parameterSetCb( block_matcher_.setSpeckleSize(param.as_int()); } else if ("speckle_range" == param_name) { block_matcher_.setSpeckleRange(param.as_int()); - } else if ("full_dp" == param_name) { - block_matcher_.setSgbmMode(param.as_bool()); + } else if ("sgbm_mode" == param_name) { + block_matcher_.setSgbmMode(param.as_int()); } else if ("P1" == param_name) { block_matcher_.setP1(param.as_double()); } else if ("P2" == param_name) {