From 547d07b3553eae79d689c86c29bde68a4ad0bf35 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 30 Jun 2023 13:55:40 +0200 Subject: [PATCH] Fix arguments when using the rclcpp_component as a regular node MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- image_view/CMakeLists.txt | 1 + image_view/src/image_view.cpp | 77 +++++++++++++++++++++++++++++- image_view/src/image_view_node.cpp | 2 +- image_view/src/utilities.cpp | 45 +++++++++++++++++ image_view/src/utilities.hpp | 35 ++++++++++++++ 5 files changed, 158 insertions(+), 2 deletions(-) create mode 100644 image_view/src/utilities.cpp create mode 100644 image_view/src/utilities.hpp diff --git a/image_view/CMakeLists.txt b/image_view/CMakeLists.txt index 27231ef73..114183322 100644 --- a/image_view/CMakeLists.txt +++ b/image_view/CMakeLists.txt @@ -57,6 +57,7 @@ add_dependencies(disparity_view ament_auto_add_executable(${PROJECT_NAME} src/image_view.cpp + src/utilities.cpp ) target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_nodes diff --git a/image_view/src/image_view.cpp b/image_view/src/image_view.cpp index 24d51fcd8..155ed4520 100644 --- a/image_view/src/image_view.cpp +++ b/image_view/src/image_view.cpp @@ -46,19 +46,94 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include +#include +#include +#include #include #include "image_view/image_view_node.hpp" +#include "utilities.hpp" int main(int argc, char ** argv) { using image_view::ImageViewNode; - rclcpp::init(argc, argv); + std::vector args = rclcpp::init_and_remove_ros_arguments(argc, argv); + + // remove program name + args.erase(args.begin()); + + std::string image_transport{"raw"}; + std::string window_name{"window"}; + bool gui{true}; + bool autosize{false}; + int width{-1}; + int height{-1}; + std::string fileformat{"frame%04i.jpg"}; + int colormap{-1}; + int min_image_value{0}; + int max_image_value{0}; + + if (image_view::has_option(args, "--image_transport")) { + image_transport = image_view::get_option(args, "--image_transport"); + } + if (image_view::has_option(args, "--window_name")) { + window_name = image_view::get_option(args, "--window_name"); + } + if (image_view::has_option(args, "--gui")) { + std::string result = image_view::get_option(args, "--gui"); + if (result.size() == 1) { + std::istringstream(result) >> gui; + } else { + std::istringstream(result) >> std::boolalpha >> gui; + } + } + if (image_view::has_option(args, "--autosize")) { + std::string result = image_view::get_option(args, "--autosize"); + if (result.size() == 1) { + std::istringstream(result) >> autosize; + } else { + std::istringstream(result) >> std::boolalpha >> autosize; + } + } + if (image_view::has_option(args, "--width")) { + width = std::atoi(image_view::get_option(args, "--width").c_str()); + } + if (image_view::has_option(args, "--height")) { + height = std::atoi(image_view::get_option(args, "--height").c_str()); + } + if (image_view::has_option(args, "--fileformat")) { + fileformat = image_view::get_option(args, "--fileformat"); + } + if (image_view::has_option(args, "--colormap")) { + colormap = std::atoi(image_view::get_option(args, "--colormap").c_str()); + } + if (image_view::has_option(args, "--min_image_value")) { + min_image_value = std::atoi(image_view::get_option(args, "--min_image_value").c_str()); + } + if (image_view::has_option(args, "--max_image_value")) { + max_image_value = std::atoi(image_view::get_option(args, "--max_image_value").c_str()); + } rclcpp::NodeOptions options; + // override default parameters with the desired transform + options.parameter_overrides( + { + {"image_transport", image_transport}, + {"window_name", window_name}, + {"gui", gui}, + {"autosize", autosize}, + {"height", height}, + {"width", width}, + {"colormap", colormap}, + {"min_image_value", min_image_value}, + {"max_image_value", max_image_value}, + {"fileformat", fileformat}, + }); + auto iv_node = std::make_shared(options); rclcpp::spin(iv_node); diff --git a/image_view/src/image_view_node.cpp b/image_view/src/image_view_node.cpp index d80e3fb07..ab620e22c 100644 --- a/image_view/src/image_view_node.cpp +++ b/image_view/src/image_view_node.cpp @@ -122,7 +122,7 @@ ImageViewNode::ImageViewNode(const rclcpp::NodeOptions & options) RCLCPP_WARN( this->get_logger(), "Topic 'image' has not been remapped! " "Typical command-line usage:\n" - "\t$ rosrun image_view image_view image:= [transport]"); + "\t$ ros2 run image_view image_view image:= --image_transport transport"); } // Default window name is the resolved topic name diff --git a/image_view/src/utilities.cpp b/image_view/src/utilities.cpp new file mode 100644 index 000000000..df5b40e3b --- /dev/null +++ b/image_view/src/utilities.cpp @@ -0,0 +1,45 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 "utilities.hpp" + +#include +#include + +namespace image_view +{ +std::string get_option(const std::vector & args, const std::string & option_name) +{ + for (auto it = args.begin(), end = args.end(); it != end; ++it) { + if (*it == option_name) { + if (it + 1 != end) { + return *(it + 1); + } + } + } + + return ""; +} + +bool has_option(const std::vector & args, const std::string & option_name) +{ + for (auto it = args.begin(), end = args.end(); it != end; ++it) { + if (*it == option_name) { + return true; + } + } + + return false; +} +} // namespace image_view diff --git a/image_view/src/utilities.hpp b/image_view/src/utilities.hpp new file mode 100644 index 000000000..c73c8c161 --- /dev/null +++ b/image_view/src/utilities.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 UTILITIES_HPP_ +#define UTILITIES_HPP_ + +#include +#include + +namespace image_view +{ +/// Get the option from a list of arguments +/// param[in] args List of arguments +/// param[in] option name to extract +/// return option value +std::string get_option(const std::vector & args, const std::string & option_name); + +/// Is the option available in the list of arguments +/// param[in] args List of arguments +/// param[in] option name to extract +/// return true if the option exists or false otherwise +bool has_option(const std::vector & args, const std::string & option_name); +} // namespace image_view +#endif // UTILITIES_HPP_