diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 6ad10c738..306df57b9 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -389,6 +389,7 @@ def __init__(self, boards, flags=0, fisheye_flags=0, pattern=Patterns.Chessboard 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. 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 ef7f26e1e..04bb27d3b 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 @@ -74,7 +45,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 +64,8 @@ Services Parameters ---------- * **encoding** (string, default:"bgr8"): Encoding type of input image topic. - * **filename_format** (string, default: "left%04i.jpg"): File name for + * **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. * **image_transport** (string, default: raw): Image transport to use. 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/include/image_view/image_saver_node.hpp b/image_view/include/image_view/image_saver_node.hpp index 5d8c76b2b..ecfd36941 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; + int fps_; bool stamped_filename_; bool save_all_image_{false}; bool save_image_service_{false}; @@ -76,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_; @@ -84,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/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 e073616ca..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(), rmw_qos_profile_sensor_data); - - 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 abd8d2576..dd4799b48 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,12 +90,14 @@ ImageSaverNode::ImageSaverNode(const rclcpp::NodeOptions & options) hints.getTransport(), rmw_qos_profile_sensor_data); // 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("left%04i.%s")); + g_format = this->declare_parameter("filename_format", std::string("frame%04i.%s")); + 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); @@ -118,6 +122,23 @@ 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( @@ -125,7 +146,8 @@ bool ImageSaverNode::saveImage( { cv::Mat image; try { - image = cv_bridge::toCvShare(image_msg, encoding_)->image; + 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( this->get_logger(), "Unable to convert %s image to %s", @@ -231,13 +253,18 @@ 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( @@ -256,19 +283,23 @@ 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