diff --git a/LICENSE b/LICENSE index 24d43da76..09078518e 100644 --- a/LICENSE +++ b/LICENSE @@ -1,3 +1,5 @@ +Portions of the code are licensed under the BSD License 2.0: + All rights reserved. Software License Agreement (BSD License 2.0) @@ -28,3 +30,210 @@ 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. + + +Portions of the code are licensed under the Apaache License 2.0: + + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. +>>>>>>> Updating copyrights and adding dual-licensing. diff --git a/image_view/CMakeLists.txt b/image_view/CMakeLists.txt index 88113d146..0d66f1ae3 100644 --- a/image_view/CMakeLists.txt +++ b/image_view/CMakeLists.txt @@ -1,36 +1,32 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.5) project(image_view) -find_package(catkin REQUIRED COMPONENTS camera_calibration_parsers cv_bridge dynamic_reconfigure image_transport message_filters message_generation nodelet rosconsole roscpp std_srvs stereo_msgs) -generate_dynamic_reconfigure_options(cfg/ImageView.cfg) - -catkin_package(CATKIN_DEPENDS dynamic_reconfigure) -find_package(Boost REQUIRED COMPONENTS thread) -find_package(OpenCV REQUIRED) - -include_directories(${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} -) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -# Extra tools -add_executable(extract_images src/nodes/extract_images.cpp) -target_link_libraries(extract_images ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} -) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -add_executable(image_saver src/nodes/image_saver.cpp) -target_link_libraries(image_saver ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} -) +find_package(ament_cmake REQUIRED) +find_package(camera_calibration_parsers REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_transport REQUIRED) +find_package(message_filters REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_srvs REQUIRED) +find_package(stereo_msgs REQUIRED) -add_executable(video_recorder src/nodes/video_recorder.cpp) -target_link_libraries(video_recorder ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} -) +find_package(Boost REQUIRED) +find_package(OpenCV REQUIRED) -install(TARGETS extract_images image_saver video_recorder - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ) # Deal with the GUI's @@ -42,49 +38,161 @@ find_package(GTK2) add_definitions(-DHAVE_GTK) include_directories(${GTK2_INCLUDE_DIRS}) -# Nodelet library -add_library(image_view src/nodelets/image_nodelet.cpp src/nodelets/disparity_nodelet.cpp src/nodelets/window_thread.cpp) -target_link_libraries(image_view ${catkin_LIBRARIES} - ${GTK_LIBRARIES} - ${GTK2_LIBRARIES} - ${OpenCV_LIBRARIES} - ${Boost_LIBRARIES} +add_library(image_view_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 + src/video_recorder_node.cpp +) +ament_target_dependencies(image_view_nodes + camera_calibration_parsers + cv_bridge + image_transport + rclcpp + rclcpp_components + sensor_msgs + std_srvs + stereo_msgs +) +target_link_libraries(image_view_nodes + ${GTK_LIBRARIES} + ${GTK2_LIBRARIES} + ${OpenCV_LIBRARIES} + ${Boost_LIBRARIES} ) -install(TARGETS image_view - DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +target_compile_definitions(image_view_nodes + PRIVATE "COMPOSITION_BUILDING_DLL" ) +rclcpp_components_register_nodes(image_view_nodes + "image_view::DisparityViewNode" +) +rclcpp_components_register_nodes(image_view_nodes + "image_view::ExtractImagesNode" +) +rclcpp_components_register_nodes(image_view_nodes + "image_view::ImageViewNode" +) +rclcpp_components_register_nodes(image_view_nodes + "image_view::ImageSaverNode" +) +rclcpp_components_register_nodes(image_view_nodes + "image_view::StereoViewNode" +) +rclcpp_components_register_nodes(image_view_nodes + "image_view::VideoRecorderNode" +) +ament_export_include_directories(include) +ament_export_interfaces(export_image_view_nodes) +ament_export_libraries(image_view_nodes) # Image viewers -add_executable(image_view_exe src/nodes/image_view.cpp) -add_dependencies(image_view_exe ${PROJECT_NAME}_gencfg) -SET_TARGET_PROPERTIES(image_view_exe PROPERTIES OUTPUT_NAME image_view) -target_link_libraries(image_view_exe ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} - ${Boost_LIBRARIES} +add_executable(disparity_view + src/disparity_view.cpp +) +ament_target_dependencies(disparity_view + rclcpp +) +target_link_libraries(disparity_view + image_view_nodes +) +add_dependencies(disparity_view + image_view_nodes ) -add_executable(disparity_view src/nodes/disparity_view.cpp) -target_link_libraries(disparity_view ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} +add_executable(image_view + src/image_view.cpp +) +ament_target_dependencies(image_view + rclcpp +) +target_link_libraries(image_view + image_view_nodes +) +add_dependencies(image_view + image_view_nodes ) -add_executable(stereo_view src/nodes/stereo_view.cpp) -target_link_libraries(stereo_view ${Boost_LIBRARIES} - ${catkin_LIBRARIES} - ${GTK_LIBRARIES} - ${GTK2_LIBRARIES} - ${OpenCV_LIBRARIES} +add_executable(stereo_view + src/stereo_view.cpp +) +ament_target_dependencies(stereo_view + rclcpp +) +target_link_libraries(stereo_view + image_view_nodes +) +add_dependencies(stereo_view + image_view_nodes ) -install(TARGETS disparity_view image_view_exe stereo_view - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# Other Tools +add_executable(extract_images + src/extract_images.cpp ) -install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +ament_target_dependencies(extract_images + rclcpp +) +target_link_libraries(extract_images + image_view_nodes +) +add_dependencies(extract_images + image_view_nodes ) -# Python programs -catkin_install_python( - PROGRAMS scripts/extract_images_sync - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +add_executable(image_saver + src/image_saver.cpp +) +ament_target_dependencies(image_saver + rclcpp +) +target_link_libraries(image_saver + image_view_nodes +) +add_dependencies(image_saver + image_view_nodes +) + +add_executable(video_recorder + src/video_recorder.cpp +) +ament_target_dependencies(video_recorder + rclcpp ) +target_link_libraries(video_recorder + image_view_nodes +) +add_dependencies(video_recorder + image_view_nodes +) + +install( + TARGETS + disparity_view + extract_images + image_saver + image_view + stereo_view + video_recorder + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(TARGETS image_view_nodes + EXPORT export_image_view_nodes + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Python programs +#catkin_install_python( +# PROGRAMS scripts/extract_images_sync +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) + +ament_package() diff --git a/image_view/COLCON_IGNORE b/image_view/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/image_view/include/image_view/disparity_view_node.hpp b/image_view/include/image_view/disparity_view_node.hpp new file mode 100644 index 000000000..389f74156 --- /dev/null +++ b/image_view/include/image_view/disparity_view_node.hpp @@ -0,0 +1,54 @@ +// 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__DISPARITY_VIEW_NODE_HPP_ +#define IMAGE_VIEW__DISPARITY_VIEW_NODE_HPP_ + +#include +#include + +#include + +#include +#include + +namespace image_view +{ + +class DisparityViewNode + : public rclcpp::Node +{ +public: + explicit DisparityViewNode(const rclcpp::NodeOptions & options); + explicit DisparityViewNode(const DisparityViewNode &) = default; + explicit DisparityViewNode(DisparityViewNode &&) = default; + DisparityViewNode & operator=(const DisparityViewNode &) = default; + DisparityViewNode & operator=(DisparityViewNode &&) = default; + ~DisparityViewNode(); + +private: + // colormap for disparities, RGB order + static unsigned char colormap[]; + + std::string window_name_; + rclcpp::Subscription::SharedPtr sub_; + cv::Mat_ disparity_color_; + bool initialized; + + void imageCb(const stereo_msgs::msg::DisparityImage::SharedPtr msg); +}; + +} // namespace image_view + +#endif // IMAGE_VIEW__DISPARITY_VIEW_NODE_HPP_ diff --git a/image_view/include/image_view/extract_images_node.hpp b/image_view/include/image_view/extract_images_node.hpp new file mode 100644 index 000000000..259bacf12 --- /dev/null +++ b/image_view/include/image_view/extract_images_node.hpp @@ -0,0 +1,87 @@ +/********************************************************************* +* 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 +#include + +namespace image_view +{ + +class ExtractImagesNode + : public rclcpp::Node +{ +public: + 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_; + boost::format 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 new file mode 100644 index 000000000..e914c5512 --- /dev/null +++ b/image_view/include/image_view/image_saver_node.hpp @@ -0,0 +1,105 @@ +/********************************************************************* +* 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__IMAGE_SAVER_HPP_ +#define IMAGE_VIEW__IMAGE_SAVER_HPP_ + +#include +#include +#include +#include + +#include + +namespace image_view +{ + +class ImageSaverNode + : public rclcpp::Node +{ +public: + ImageSaverNode(const rclcpp::NodeOptions & options); + +private: + boost::format g_format; + bool save_all_image, save_image_service; + std::string encoding; + bool request_start_end; + bool is_first_image_; + bool has_camera_info_; + size_t count_; + rclcpp::Time start_time_; + rclcpp::Time end_time_; + image_transport::CameraSubscriber cam_sub_; + image_transport::Subscriber image_sub_; + rclcpp::Service::SharedPtr save_srv_; + rclcpp::Service::SharedPtr start_srv_; + rclcpp::Service::SharedPtr end_srv_; + + bool saveImage(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, std::string & filename); + bool service( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + bool callbackStartSave( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + bool callbackEndSave( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void callbackWithoutCameraInfo(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg); + void callbackWithCameraInfo( + const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info); +}; + +} // namespace image_view + +#endif // IMAGE_VIEW__IMAGE_SAVER_HPP_ diff --git a/image_view/include/image_view/image_view_node.hpp b/image_view/include/image_view/image_view_node.hpp new file mode 100644 index 000000000..1ae90ffd0 --- /dev/null +++ b/image_view/include/image_view/image_view_node.hpp @@ -0,0 +1,83 @@ +// 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__IMAGE_VIEW_NODE_HPP_ +#define IMAGE_VIEW__IMAGE_VIEW_NODE_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace image_view +{ + +class ThreadSafeImage +{ + std::mutex mutex_; + std::condition_variable condition_; + cv_bridge::CvImageConstPtr image_; + +public: + void set(cv_bridge::CvImageConstPtr image); + cv_bridge::CvImageConstPtr get(); + cv_bridge::CvImageConstPtr pop(); +}; + +class ImageViewNode + : public rclcpp::Node +{ +public: + explicit ImageViewNode(const rclcpp::NodeOptions & options); + explicit ImageViewNode(const ImageViewNode &) = default; + explicit ImageViewNode(ImageViewNode &&) = default; + ImageViewNode & operator=(const ImageViewNode &) = default; + ImageViewNode & operator=(ImageViewNode &&) = default; + ~ImageViewNode(); + +private: + ThreadSafeImage queued_image_, shown_image_; + bool autosize_; + int window_height_, window_width_; + bool g_gui; + boost::format filename_format_; + image_transport::Subscriber sub_; + int count_; + double min_image_value_, max_image_value_; + int colormap_; + rclcpp::TimerBase::SharedPtr gui_timer_; + std::shared_ptr> pub_; + std::string window_name_; + std::thread window_thread_; + + void imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & msg); + static void mouseCb(int event, int x, int y, int flags, void * param); + static void guiCb(); + void windowThread(); + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector &); + std::mutex param_mutex_; +}; + +} // namespace image_view + +#endif // IMAGE_VIEW__IMAGE_VIEW_NODE_HPP_ diff --git a/image_view/include/image_view/stereo_view_node.hpp b/image_view/include/image_view/stereo_view_node.hpp new file mode 100644 index 000000000..adf61cdf9 --- /dev/null +++ b/image_view/include/image_view/stereo_view_node.hpp @@ -0,0 +1,180 @@ +/********************************************************************* +* 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__STEREO_VIEW_HPP_ +#define IMAGE_VIEW__STEREO_VIEW_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace image_view +{ + +using sensor_msgs::msg::Image; +using stereo_msgs::msg::DisparityImage; +using message_filters::sync_policies::ExactTime; +using message_filters::sync_policies::ApproximateTime; + +class StereoViewNode + : public rclcpp::Node +{ +public: + explicit StereoViewNode(const rclcpp::NodeOptions & options); + explicit StereoViewNode(const StereoViewNode &) = default; + explicit StereoViewNode(StereoViewNode &&) = default; + StereoViewNode & operator=(const StereoViewNode &) = default; + StereoViewNode & operator=(StereoViewNode &&) = default; + ~StereoViewNode(); + +private: + using ExactPolicy = ExactTime; + using ApproximatePolicy = ApproximateTime; + using ExactSync = message_filters::Synchronizer; + using ApproximateSync = message_filters::Synchronizer; + + image_transport::SubscriberFilter left_sub_, right_sub_; + message_filters::Subscriber disparity_sub_; + std::shared_ptr exact_sync_; + std::shared_ptr approximate_sync_; + int queue_size_; + + Image::ConstSharedPtr last_left_msg_, last_right_msg_; + cv::Mat last_left_image_, last_right_image_; + cv::Mat_ disparity_color_; + std::mutex image_mutex_; + + boost::format filename_format_; + int save_count_; + + rclcpp::TimerBase::SharedPtr check_synced_timer_; + int left_received_, right_received_, disp_received_, all_received_; + + // colormap for disparities, RGB + static constexpr unsigned char colormap[768] = + { + 150, 150, 150, 107, 0, 12, 106, 0, 18, 105, 0, 24, 103, 0, 30, + 102, 0, 36, 101, 0, 42, 99, 0, 48, 98, 0, 54, 97, 0, 60, + 96, 0, 66, 94, 0, 72, 93, 0, 78, 92, 0, 84, 91, 0, 90, + 89, 0, 96, 88, 0, 102, 87, 0, 108, 85, 0, 114, 84, 0, 120, + 83, 0, 126, 82, 0, 131, 80, 0, 137, 79, 0, 143, 78, 0, 149, + 77, 0, 155, 75, 0, 161, 74, 0, 167, 73, 0, 173, 71, 0, 179, + 70, 0, 185, 69, 0, 191, 68, 0, 197, 66, 0, 203, 65, 0, 209, + 64, 0, 215, 62, 0, 221, 61, 0, 227, 60, 0, 233, 59, 0, 239, + 57, 0, 245, 56, 0, 251, 55, 0, 255, 54, 0, 255, 52, 0, 255, + 51, 0, 255, 50, 0, 255, 48, 0, 255, 47, 0, 255, 46, 0, 255, + 45, 0, 255, 43, 0, 255, 42, 0, 255, 41, 0, 255, 40, 0, 255, + 38, 0, 255, 37, 0, 255, 36, 0, 255, 34, 0, 255, 33, 0, 255, + 32, 0, 255, 31, 0, 255, 29, 0, 255, 28, 0, 255, 27, 0, 255, + 26, 0, 255, 24, 0, 255, 23, 0, 255, 22, 0, 255, 20, 0, 255, + 19, 0, 255, 18, 0, 255, 17, 0, 255, 15, 0, 255, 14, 0, 255, + 13, 0, 255, 11, 0, 255, 10, 0, 255, 9, 0, 255, 8, 0, 255, + 6, 0, 255, 5, 0, 255, 4, 0, 255, 3, 0, 255, 1, 0, 255, + 0, 4, 255, 0, 10, 255, 0, 16, 255, 0, 22, 255, 0, 28, 255, + 0, 34, 255, 0, 40, 255, 0, 46, 255, 0, 52, 255, 0, 58, 255, + 0, 64, 255, 0, 70, 255, 0, 76, 255, 0, 82, 255, 0, 88, 255, + 0, 94, 255, 0, 100, 255, 0, 106, 255, 0, 112, 255, 0, 118, 255, + 0, 124, 255, 0, 129, 255, 0, 135, 255, 0, 141, 255, 0, 147, 255, + 0, 153, 255, 0, 159, 255, 0, 165, 255, 0, 171, 255, 0, 177, 255, + 0, 183, 255, 0, 189, 255, 0, 195, 255, 0, 201, 255, 0, 207, 255, + 0, 213, 255, 0, 219, 255, 0, 225, 255, 0, 231, 255, 0, 237, 255, + 0, 243, 255, 0, 249, 255, 0, 255, 255, 0, 255, 249, 0, 255, 243, + 0, 255, 237, 0, 255, 231, 0, 255, 225, 0, 255, 219, 0, 255, 213, + 0, 255, 207, 0, 255, 201, 0, 255, 195, 0, 255, 189, 0, 255, 183, + 0, 255, 177, 0, 255, 171, 0, 255, 165, 0, 255, 159, 0, 255, 153, + 0, 255, 147, 0, 255, 141, 0, 255, 135, 0, 255, 129, 0, 255, 124, + 0, 255, 118, 0, 255, 112, 0, 255, 106, 0, 255, 100, 0, 255, 94, + 0, 255, 88, 0, 255, 82, 0, 255, 76, 0, 255, 70, 0, 255, 64, + 0, 255, 58, 0, 255, 52, 0, 255, 46, 0, 255, 40, 0, 255, 34, + 0, 255, 28, 0, 255, 22, 0, 255, 16, 0, 255, 10, 0, 255, 4, + 2, 255, 0, 8, 255, 0, 14, 255, 0, 20, 255, 0, 26, 255, 0, + 32, 255, 0, 38, 255, 0, 44, 255, 0, 50, 255, 0, 56, 255, 0, + 62, 255, 0, 68, 255, 0, 74, 255, 0, 80, 255, 0, 86, 255, 0, + 92, 255, 0, 98, 255, 0, 104, 255, 0, 110, 255, 0, 116, 255, 0, + 122, 255, 0, 128, 255, 0, 133, 255, 0, 139, 255, 0, 145, 255, 0, + 151, 255, 0, 157, 255, 0, 163, 255, 0, 169, 255, 0, 175, 255, 0, + 181, 255, 0, 187, 255, 0, 193, 255, 0, 199, 255, 0, 205, 255, 0, + 211, 255, 0, 217, 255, 0, 223, 255, 0, 229, 255, 0, 235, 255, 0, + 241, 255, 0, 247, 255, 0, 253, 255, 0, 255, 251, 0, 255, 245, 0, + 255, 239, 0, 255, 233, 0, 255, 227, 0, 255, 221, 0, 255, 215, 0, + 255, 209, 0, 255, 203, 0, 255, 197, 0, 255, 191, 0, 255, 185, 0, + 255, 179, 0, 255, 173, 0, 255, 167, 0, 255, 161, 0, 255, 155, 0, + 255, 149, 0, 255, 143, 0, 255, 137, 0, 255, 131, 0, 255, 126, 0, + 255, 120, 0, 255, 114, 0, 255, 108, 0, 255, 102, 0, 255, 96, 0, + 255, 90, 0, 255, 84, 0, 255, 78, 0, 255, 72, 0, 255, 66, 0, + 255, 60, 0, 255, 54, 0, 255, 48, 0, 255, 42, 0, 255, 36, 0, + 255, 30, 0, 255, 24, 0, 255, 18, 0, 255, 12, 0, 255, 6, 0, + 255, 0, 0, + }; + + void imageCb( + const Image::ConstSharedPtr & left, const Image::ConstSharedPtr & right, + const DisparityImage::ConstSharedPtr & disparity_msg); + void saveImage(const char * prefix, const cv::Mat & image); + static void mouseCb(int event, int x, int y, int flags, void * param); + void checkInputsSynchronized(); + + static inline void increment(int * value) + { + ++(*value); + } +}; + +} + +#endif // IMAGE_VIEW__STEREO_VIEW_HPP_ diff --git a/image_view/include/image_view/video_recorder_node.hpp b/image_view/include/image_view/video_recorder_node.hpp new file mode 100644 index 000000000..00c27a0d1 --- /dev/null +++ b/image_view/include/image_view/video_recorder_node.hpp @@ -0,0 +1,61 @@ +// Copyright 2012, 2013, 2019 Open Source Robotics Foundation, 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__VIDEO_RECORDER_NODE_HPP_ +#define IMAGE_VIEW__VIDEO_RECORDER_NODE_HPP_ + +#include +#include + +#include + +#include +#include + +namespace image_view +{ + +class VideoRecorderNode + : public rclcpp::Node +{ +public: + explicit VideoRecorderNode(const rclcpp::NodeOptions & options); + explicit VideoRecorderNode(const VideoRecorderNode &) = default; + explicit VideoRecorderNode(VideoRecorderNode &&) = default; + VideoRecorderNode & operator=(const VideoRecorderNode &) = default; + VideoRecorderNode & operator=(VideoRecorderNode &&) = default; + ~VideoRecorderNode(); + +private: + cv::VideoWriter outputVideo; + + int g_count; + rclcpp::Time g_last_wrote_time; + std::string encoding; + std::string codec; + int fps; + double min_depth_range; + double max_depth_range; + bool use_dynamic_range; + int colormap; + image_transport::Subscriber sub_image; + bool recording_started; + std::string filename; + + void callback(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg); +}; + +} // namespace image_view + +#endif // IMAGE_VIEW__VIDEO_RECORDER_NODE_HPP_ diff --git a/image_view/nodelet_plugins.xml b/image_view/nodelet_plugins.xml deleted file mode 100644 index b6bd0fcb4..000000000 --- a/image_view/nodelet_plugins.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - Nodelet to view a sensor_msgs/Image topic - - - - Nodelet to view a stereo_msgs/DisparityImage topic - - - diff --git a/image_view/package.xml b/image_view/package.xml index 1e3becb95..3cad7ae9f 100644 --- a/image_view/package.xml +++ b/image_view/package.xml @@ -1,4 +1,6 @@ - + + + image_view 1.12.23 @@ -12,37 +14,23 @@ BSD http://www.ros.org/wiki/image_view - - - - - - catkin + ament_cmake - rostest + camera_calibration_parsers + cv_bridge + gtk2 + image_transport + message_filters + rclcpp + sensor_msgs + std_srvs + stereo_msgs - camera_calibration_parsers - cv_bridge - dynamic_reconfigure - gtk2 - image_transport - message_filters - message_generation - nodelet - rosconsole - roscpp - sensor_msgs - std_srvs - stereo_msgs + ament_lint_auto + ament_lint_common - camera_calibration_parsers - cv_bridge - dynamic_reconfigure - gtk2 - image_transport - message_filters - nodelet - rosconsole - roscpp - std_srvs + + ament_cmake + + diff --git a/image_view/src/nodes/disparity_view.cpp b/image_view/src/disparity_view.cpp similarity index 67% rename from image_view/src/nodes/disparity_view.cpp rename to image_view/src/disparity_view.cpp index d013b22c6..0d245b871 100644 --- a/image_view/src/nodes/disparity_view.cpp +++ b/image_view/src/disparity_view.cpp @@ -31,24 +31,36 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include + +// 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 "image_view/disparity_view_node.hpp" + +using image_view::DisparityViewNode; int main(int argc, char **argv) { - ros::init(argc, argv, "disparity_view", ros::init_options::AnonymousName); - if (ros::names::remap("image") == "image") { - ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n" - "\t$ rosrun image_view disparity_view image:="); - } - - nodelet::Loader manager(false); - nodelet::M_string remappings; - nodelet::V_string my_argv(argv + 1, argv + argc); - my_argv.push_back("--shutdown-on-close"); // Internal + rclcpp::init(argc, argv); - manager.load(ros::this_node::getName(), "image_view/disparity", remappings, my_argv); + rclcpp::NodeOptions options; + auto dv_node = std::make_shared(options); - ros::spin(); + rclcpp::spin(dv_node); return 0; } diff --git a/image_view/src/nodelets/disparity_nodelet.cpp b/image_view/src/disparity_view_node.cpp similarity index 70% rename from image_view/src/nodelets/disparity_nodelet.cpp rename to image_view/src/disparity_view_node.cpp index e177a8762..3443b17f7 100644 --- a/image_view/src/nodelets/disparity_nodelet.cpp +++ b/image_view/src/disparity_view_node.cpp @@ -31,107 +31,82 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include -#include -#include -#include -#include "window_thread.h" -#ifdef HAVE_GTK -#include +// 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. -// Platform-specific workaround for #3026: image_view doesn't close when -// closing image window. On platforms using GTK+ we connect this to the -// window's "destroy" event so that image_view exits. -static void destroyNode(GtkWidget *widget, gpointer data) -{ - exit(0); -} - -static void destroyNodelet(GtkWidget *widget, gpointer data) -{ - // We can't actually unload the nodelet from here, but we can at least - // unsubscribe from the image topic. - reinterpret_cast(data)->shutdown(); -} -#endif +#include +#include +#include +#include -namespace image_view { +#include "image_view/disparity_view_node.hpp" -class DisparityNodelet : public nodelet::Nodelet -{ - // colormap for disparities, RGB order - static unsigned char colormap[]; +#ifdef HAVE_GTK - std::string window_name_; - ros::Subscriber sub_; - cv::Mat_ disparity_color_; - bool initialized; - - virtual void onInit(); - - void imageCb(const stereo_msgs::DisparityImageConstPtr& msg); +#include -public: - ~DisparityNodelet(); -}; +#endif -DisparityNodelet::~DisparityNodelet() +namespace image_view { - cv::destroyWindow(window_name_); -} -void DisparityNodelet::onInit() +DisparityViewNode::DisparityViewNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("disparity_view_node", options) { - initialized = false; - ros::NodeHandle nh = getNodeHandle(); - ros::NodeHandle local_nh = getPrivateNodeHandle(); - const std::vector& argv = getMyArgv(); + std::string topic = rclcpp::expand_topic_or_service_name("image", this->get_name(), this->get_namespace()); - // Internal option, should be used only by image_view nodes - bool shutdown_on_close = std::find(argv.begin(), argv.end(), - "--shutdown-on-close") != argv.end(); + if (topic == "image") { + RCLCPP_WARN( + this->get_logger(), "Topic 'image' has not been remapped! Typical command-line usage:\n" + "\t$ rosrun image_view disparity_view image:="); + } + + initialized = false; // Default window name is the resolved topic name - std::string topic = nh.resolveName("image"); - local_nh.param("window_name", window_name_, topic); + window_name_ = this->declare_parameter("window_name", topic); + // bool autosize = this->declare_parameter("autosize", false); - bool autosize; - local_nh.param("autosize", autosize, false); + // cv::namedWindow(window_name_, autosize ? cv::WND_PROP_AUTOSIZE : 0); - //cv::namedWindow(window_name_, autosize ? cv::WND_PROP_AUTOSIZE : 0); -#if CV_MAJOR_VERSION ==2 -#ifdef HAVE_GTK - // Register appropriate handler for when user closes the display window - GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) ); - if (shutdown_on_close) - g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL); - else - g_signal_connect(widget, "destroy", G_CALLBACK(destroyNodelet), &sub_); -#endif - // Start the OpenCV window thread so we don't have to waitKey() somewhere - startWindowThread(); -#endif + sub_ = this->create_subscription( + topic, rclcpp::QoS(10), std::bind(&DisparityViewNode::imageCb, this, std::placeholders::_1)); +} - sub_ = nh.subscribe(topic, 1, &DisparityNodelet::imageCb, this); +DisparityViewNode::~DisparityViewNode() +{ + cv::destroyWindow(window_name_); } -void DisparityNodelet::imageCb(const stereo_msgs::DisparityImageConstPtr& msg) +void DisparityViewNode::imageCb(const stereo_msgs::msg::DisparityImage::SharedPtr msg) { // Check for common errors in input if (msg->min_disparity == 0.0 && msg->max_disparity == 0.0) { - NODELET_ERROR_THROTTLE(30, "Disparity image fields min_disparity and " - "max_disparity are not set"); + RCLCPP_ERROR_EXPRESSION( + this->get_logger(), (static_cast(this->now().seconds()) % 30 == 0), + "Disparity image fields min_disparity and max_disparity are not set"); return; } if (msg->image.encoding != sensor_msgs::image_encodings::TYPE_32FC1) { - NODELET_ERROR_THROTTLE(30, "Disparity image must be 32-bit floating point " - "(encoding '32FC1'), but has encoding '%s'", - msg->image.encoding.c_str()); + RCLCPP_ERROR_EXPRESSION( + this->get_logger(), (static_cast(this->now().seconds()) % 30 == 0), + "Disparity image must be 32-bit floating point (encoding '32FC1'), but has encoding '%s'", + msg->image.encoding.c_str()); return; } @@ -139,19 +114,23 @@ void DisparityNodelet::imageCb(const stereo_msgs::DisparityImageConstPtr& msg) cv::namedWindow(window_name_, false ? cv::WND_PROP_AUTOSIZE : 0); initialized = true; } + // Colormap and display the disparity image float min_disparity = msg->min_disparity; float max_disparity = msg->max_disparity; float multiplier = 255.0f / (max_disparity - min_disparity); - const cv::Mat_ dmat(msg->image.height, msg->image.width, - (float*)&msg->image.data[0], msg->image.step); + const cv::Mat_ dmat( + msg->image.height, msg->image.width, + (float*)&msg->image.data[0], msg->image.step); disparity_color_.create(msg->image.height, msg->image.width); for (int row = 0; row < disparity_color_.rows; ++row) { const float* d = dmat[row]; - cv::Vec3b *disparity_color = disparity_color_[row], - *disparity_color_end = disparity_color + disparity_color_.cols; + cv::Vec3b *disparity_color = + disparity_color_[row], *disparity_color_end = + disparity_color + disparity_color_.cols; + for (; disparity_color < disparity_color_end; ++disparity_color, ++d) { int index = (*d - min_disparity) * multiplier + 0.5; index = std::min(255, std::max(0, index)); @@ -173,7 +152,7 @@ void DisparityNodelet::imageCb(const stereo_msgs::DisparityImageConstPtr& msg) cv::waitKey(10); } -unsigned char DisparityNodelet::colormap[768] = +unsigned char DisparityViewNode::colormap[768] = { 150, 150, 150, 107, 0, 12, 106, 0, 18, @@ -432,8 +411,7 @@ unsigned char DisparityNodelet::colormap[768] = 255, 0, 0, }; -} // namespace image_view +} // namespace image_view -// Register the nodelet -#include -PLUGINLIB_EXPORT_CLASS( image_view::DisparityNodelet, nodelet::Nodelet) +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_view::DisparityViewNode) diff --git a/image_view/src/extract_images.cpp b/image_view/src/extract_images.cpp new file mode 100644 index 000000000..255b6032e --- /dev/null +++ b/image_view/src/extract_images.cpp @@ -0,0 +1,68 @@ +/********************************************************************* +* 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; + options.append_parameter_override("transport", (argc > 1) ? argv[1] : "raw"); + 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 new file mode 100644 index 000000000..b9641bca0 --- /dev/null +++ b/image_view/src/extract_images_node.cpp @@ -0,0 +1,136 @@ +/********************************************************************* +* 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 +#include + +#include + +#include +#include + +#include "image_view/extract_images_node.hpp" + +namespace image_view +{ + +ExtractImagesNode::ExtractImagesNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("extract_images_node", options), + filename_format_(""), count_(0), _time(this->now()) +{ + auto topic = rclcpp::expand_topic_or_service_name("image", this->get_name(), this->get_namespace()); + + std::string transport = this->declare_parameter("transport", std::string("raw")); + + sub_ = image_transport::create_subscription( + this, topic, std::bind( + &ExtractImagesNode::image_cb, this, std::placeholders::_1), + transport); + + 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$ ./extract_images image:= [transport]"); + } + + std::string format_string = + this->declare_parameter("filename_format", std::string("frame%04i.jpg")); + filename_format_.parse(format_string); + + double sec_per_frame_ = this->declare_parameter("sec_per_frame", 0.1); + + 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(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 = (filename_format_ % count_).str(); + + 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 + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_view::ExtractImagesNode) diff --git a/image_view/src/nodelets/window_thread.cpp b/image_view/src/image_saver.cpp similarity index 66% rename from image_view/src/nodelets/window_thread.cpp rename to image_view/src/image_saver.cpp index 8e1c8eb48..ef35b5c9d 100644 --- a/image_view/src/nodelets/window_thread.cpp +++ b/image_view/src/image_saver.cpp @@ -31,22 +31,35 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include "window_thread.h" -#include -#include -namespace { -void startWindowThreadLocal() { - cv::startWindowThread(); -} -} +// 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 -namespace image_view { +#include "image_view/image_saver_node.hpp" -void startWindowThread() +int main(int argc, char ** argv) { - static boost::once_flag cv_thread_flag = BOOST_ONCE_INIT; - boost::call_once(&startWindowThreadLocal, cv_thread_flag); -} + using image_view::ImageSaverNode; + + rclcpp::init(argc, argv); -} // namespace image_view + rclcpp::NodeOptions options; + auto is_node = std::make_shared(options); + + rclcpp::spin(is_node); +} diff --git a/image_view/src/image_saver_node.cpp b/image_view/src/image_saver_node.cpp new file mode 100644 index 000000000..38823cddd --- /dev/null +++ b/image_view/src/image_saver_node.cpp @@ -0,0 +1,262 @@ +/********************************************************************* +* 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 "image_view/image_saver_node.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace image_view +{ + +using std::placeholders::_1; +using std::placeholders::_2; +using std::placeholders::_3; + +ImageSaverNode::ImageSaverNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("image_saver_node", options), + is_first_image_(true), has_camera_info_(false), count_(0) +{ + auto topic = rclcpp::expand_topic_or_service_name("image", this->get_name(), this->get_namespace()); + + // Useful when CameraInfo is being published + cam_sub_ = image_transport::create_camera_subscription( + this, topic, std::bind( + &ImageSaverNode::callbackWithCameraInfo, this, std::placeholders::_1, std::placeholders::_2), + "raw"); + + // Useful when CameraInfo is not being published + image_sub_ = image_transport::create_subscription( + this, topic, std::bind( + &ImageSaverNode::callbackWithoutCameraInfo, this, std::placeholders::_1), + "raw"); + + std::string format_string; + format_string = this->declare_parameter("filename_format", std::string("left%04i.%s")); + encoding = this->declare_parameter("encoding", std::string("bgr8")); + save_all_image = this->declare_parameter("save_all_image", true); + g_format.parse(format_string); + + save_srv_ = this->create_service( + "save", std::bind(&ImageSaverNode::service, this, _1, _2, _3)); + start_srv_ = this->create_service( + "start", std::bind(&ImageSaverNode::callbackStartSave, this, _1, _2, _3)); + end_srv_ = this->create_service( + "end", std::bind(&ImageSaverNode::callbackEndSave, this, _1, _2, _3)); +} + +bool ImageSaverNode::saveImage(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, std::string & filename) +{ + cv::Mat image; + try { + image = cv_bridge::toCvShare(image_msg, encoding)->image; + } catch(cv_bridge::Exception) { + RCLCPP_ERROR( + this->get_logger(), "Unable to convert %s image to %s", + image_msg->encoding.c_str(), encoding.c_str()); + return false; + } + + if (!image.empty()) { + try { + filename = (g_format).str(); + } catch (...) { + g_format.clear(); + } + + try { + filename = (g_format % count_).str(); + } catch (...) { + g_format.clear(); + } + + try { + filename = (g_format % count_ % "jpg").str(); + } catch (...) { + g_format.clear(); + } + + if (save_all_image || save_image_service ) { + cv::imwrite(filename, image); + RCLCPP_INFO(this->get_logger(), "Saved image %s", filename.c_str()); + + save_image_service = false; + } else { + return false; + } + } else { + RCLCPP_WARN(this->get_logger(), "Couldn't save image, no data!"); + return false; + } + + return true; +} + +bool ImageSaverNode::service( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request_header; + (void)request; + (void)response; + save_image_service = true; + return true; +} + +bool ImageSaverNode::callbackStartSave( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request_header; + (void)request; + RCLCPP_INFO(this->get_logger(), "Received start saving request"); + start_time_ = this->now(); + end_time_ = rclcpp::Time(0); + + response->success = true; + return true; +} + +bool ImageSaverNode::callbackEndSave( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request_header; + (void)request; + RCLCPP_INFO(this->get_logger(), "Received end saving request"); + end_time_ = this->now(); + + response->success = true; + return true; +} + +void ImageSaverNode::callbackWithoutCameraInfo(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg) +{ + if (is_first_image_) { + is_first_image_ = false; + + // Wait a tiny bit to see whether callbackWithCameraInfo is called + rclcpp::sleep_for(std::chrono::milliseconds(1)); + } + + if (has_camera_info_) { + return; + } + + // saving flag priority: + // 1. request by service. + // 2. request by topic about start and end. + // 3. flag 'save_all_image'. + if (!save_image_service && request_start_end) { + if (start_time_ == rclcpp::Time(0)) { + return; + } else if (start_time_ > image_msg->header.stamp) { + return; // wait for message which comes after start_time + } else if ((end_time_ != rclcpp::Time(0)) && (end_time_ < image_msg->header.stamp)) { + return; // skip message which comes after end_time + } + } + + // save the image + std::string filename; + if (!saveImage(image_msg, filename)) { + return; + } + + count_++; +} + +void ImageSaverNode::callbackWithCameraInfo( + const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info) +{ + has_camera_info_ = true; + + if (!save_image_service && request_start_end) { + if (start_time_ == rclcpp::Time(0)) { + return; + } else if (start_time_ > image_msg->header.stamp) { + return; // wait for message which comes after start_time + } else if ((end_time_ != rclcpp::Time(0)) && (end_time_ < image_msg->header.stamp)) { + return; // skip message which comes after end_time + } + } + + // 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); + } + + count_++; +} + +} // namespace image_view + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_view::ImageSaverNode) diff --git a/image_view/src/nodelets/window_thread.h b/image_view/src/image_view.cpp similarity index 65% rename from image_view/src/nodelets/window_thread.h rename to image_view/src/image_view.cpp index 9418e79c5..d3753f256 100644 --- a/image_view/src/nodelets/window_thread.h +++ b/image_view/src/image_view.cpp @@ -1,13 +1,13 @@ /********************************************************************* * 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 @@ -17,7 +17,7 @@ * * 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 @@ -31,14 +31,39 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef IMAGE_VIEW_WINDOW_THREAD_H -#define IMAGE_VIEW_WINDOW_THREAD_H -namespace image_view { +// 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. -// Makes absolutely sure we only start the OpenCV window thread once -void startWindowThread(); +#include -} // namespace image_view +#include -#endif +#include "image_view/image_view_node.hpp" + +int main(int argc, char **argv) +{ + using image_view::ImageViewNode; + + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto iv_node = std::make_shared(options); + + rclcpp::spin(iv_node); + + rclcpp::shutdown(); + + return 0; +} diff --git a/image_view/src/image_view_node.cpp b/image_view/src/image_view_node.cpp new file mode 100644 index 000000000..fed424598 --- /dev/null +++ b/image_view/src/image_view_node.cpp @@ -0,0 +1,292 @@ +/********************************************************************* +* 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 + +#include + +#include + +#include +#include +#include +#include + +#include "image_view/image_view_node.hpp" + +namespace image_view +{ + +void ThreadSafeImage::set(cv_bridge::CvImageConstPtr image) +{ + std::lock_guard lock(mutex_); + image_ = image; + condition_.notify_one(); +} + +cv_bridge::CvImageConstPtr ThreadSafeImage::get() +{ + std::lock_guard lock(mutex_); + return image_; +} + +cv_bridge::CvImageConstPtr ThreadSafeImage::pop() +{ + cv_bridge::CvImageConstPtr image; + + { + std::unique_lock lock(mutex_); + + condition_.wait_for(lock, std::chrono::milliseconds(100), [this] { + return !image_; + }); + + image = std::move(image_); + } + + return image; +} + +ImageViewNode::ImageViewNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("image_view_node", options) +{ + auto transport = this->declare_parameter("image_transport", "raw"); + RCLCPP_INFO(this->get_logger(), "Using transport \"%s\"", transport.c_str()); + + std::string topic = rclcpp::expand_topic_or_service_name("image", this->get_name(), this->get_namespace()); + + image_transport::TransportHints hints(this, transport); + pub_ = this->create_publisher("output", 1); + sub_ = image_transport::create_subscription(this, topic, std::bind(&ImageViewNode::imageCb, 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(), "Topic 'image' has not been remapped! Typical command-line usage:\n" + "\t$ rosrun image_view image_view image:= [transport]"); + } + + // Default window name is the resolved topic name + window_name_ = this->declare_parameter("window_name", topic); + g_gui = this->declare_parameter("gui", true); // gui/no_gui mode + autosize_ = this->declare_parameter("autosize", false); + window_height_ = this->declare_parameter("height", -1); + window_width_ = this->declare_parameter("width", -1); + std::string format_string = this->declare_parameter("filename_format", std::string("frame%04i.jpg")); + filename_format_.parse(format_string); + + colormap_ = this->declare_parameter("colormap", -1); + min_image_value_ = this->declare_parameter("min_image_value", 0); + max_image_value_ = this->declare_parameter("max_image_value", 0); + + if(g_gui) { + // Since cv::startWindowThread() triggers a crash in cv::waitKey() + // if OpenCV is compiled against GTK, we call cv::waitKey() from + // the ROS event loop periodically, instead. + gui_timer_ = this->create_wall_timer(std::chrono::milliseconds(100), &ImageViewNode::guiCb); + + window_thread_ = std::thread(&ImageViewNode::windowThread, this); + } + + this->set_on_parameters_set_callback(std::bind(&ImageViewNode::paramCallback, this, std::placeholders::_1)); +} + +ImageViewNode::~ImageViewNode() +{ + if (window_thread_.joinable()) { + window_thread_.join(); + } +} + +void ImageViewNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & msg) +{ + // We want to scale floating point images so that they display nicely + bool do_dynamic_scaling = (msg->encoding.find("F") != std::string::npos); + + // Convert to OpenCV native BGR color + try { + cv_bridge::CvtColorForDisplayOptions options; + options.do_dynamic_scaling = do_dynamic_scaling; + + { + std::lock_guard lock(param_mutex_); + options.colormap = colormap_; + + // Set min/max value for scaling to visualize depth/float image. + if (min_image_value_ == max_image_value_) { + // Not specified by rosparam, then set default value. + // Because of current sensor limitation, we use 10m as default of max range of depth + // with consistency to the configuration in rqt_image_view. + options.min_image_value = 0; + + if (msg->encoding == "32FC1") { + options.max_image_value = 10; // 10 [m] + } else if (msg->encoding == "16UC1") { + options.max_image_value = 10 * 1000; // 10 * 1000 [mm] + } + } else { + options.min_image_value = min_image_value_; + options.max_image_value = max_image_value_; + } + } + + queued_image_.set( + cv_bridge::cvtColorForDisplay( + cv_bridge::toCvShare(msg), "bgr8", options)); + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR_EXPRESSION( + this->get_logger(), (static_cast(this->now().seconds()) % 30 == 0), + "Unable to convert '%s' image for display: '%s'", + msg->encoding.c_str(), e.what()); + } + + if (pub_->get_subscription_count() > 0) { + pub_->publish(*msg); + } +} + +void ImageViewNode::guiCb() +{ + // Process pending GUI events and return immediately + cv::waitKey(1); +} + +void ImageViewNode::mouseCb(int event, int /* x */, int /* y */, int /* flags */, void * param) +{ + ImageViewNode *this_ = reinterpret_cast(param); + + if (event == cv::EVENT_LBUTTONDOWN) { + RCLCPP_WARN_ONCE(this_->get_logger(), "Left-clicking no longer saves images. Right-click instead."); + return; + } + + if (event != cv::EVENT_RBUTTONDOWN) { + return; + } + + cv_bridge::CvImageConstPtr image(this_->shown_image_.get()); + + if (!image) { + RCLCPP_WARN(this_->get_logger(), "Couldn't save image, no data!"); + return; + } + + std::string filename = (this_->filename_format_ % this_->count_).str(); + + if (cv::imwrite(filename, image->image)) { + RCLCPP_INFO(this_->get_logger(), "Saved image %s", filename.c_str()); + this_->count_++; + } else { + /// @todo Show full path, ask if user has permission to write there + RCLCPP_ERROR(this_->get_logger(), "Failed to save image."); + } +} + +void ImageViewNode::windowThread() +{ + cv::namedWindow(window_name_, autosize_ ? (cv::WINDOW_AUTOSIZE | cv::WINDOW_KEEPRATIO | cv::WINDOW_GUI_EXPANDED) : 0); + cv::setMouseCallback(window_name_, &ImageViewNode::mouseCb, this); + + if(!autosize_ && window_width_ > -1 && window_height_ > -1) { + cv::resizeWindow(window_name_, window_width_, window_height_); + } + + while(rclcpp::ok()) { + cv_bridge::CvImageConstPtr image(queued_image_.pop()); + + if (cv::getWindowProperty(window_name_, 1) < 0) { + break; + } + + if (image) { + cv::imshow(window_name_, image->image); + shown_image_.set(image); + cv::waitKey(1); + } + } + + cv::destroyWindow(window_name_); + + if (rclcpp::ok()) { + rclcpp::shutdown(); + } +} + +rcl_interfaces::msg::SetParametersResult +ImageViewNode::paramCallback(const std::vector & parameters) +{ + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + + for (const auto & parameter : parameters) { + if (parameter.get_name() == "colormap") { + std::lock_guard lock(param_mutex_); + colormap_ = parameter.as_int(); + break; + } else if (parameter.get_name() == "min_image_value") { + std::lock_guard lock(param_mutex_); + min_image_value_ = parameter.as_int(); + break; + } else if (parameter.get_name() == "max_image_value") { + std::lock_guard lock(param_mutex_); + max_image_value_ = parameter.as_int(); + break; + } + } + + return result; +} + +} // namespace image_view + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_view::ImageViewNode) diff --git a/image_view/src/nodelets/image_nodelet.cpp b/image_view/src/nodelets/image_nodelet.cpp deleted file mode 100644 index 57467d739..000000000 --- a/image_view/src/nodelets/image_nodelet.cpp +++ /dev/null @@ -1,265 +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. -*********************************************************************/ -#include -#include -#include - -#include -#include -#include "window_thread.h" - -#include -#include -#include - - -namespace image_view { - -class ThreadSafeImage -{ - boost::mutex mutex_; - boost::condition_variable condition_; - cv::Mat image_; - -public: - void set(const cv::Mat& image); - - cv::Mat get(); - - cv::Mat pop(); -}; - -void ThreadSafeImage::set(const cv::Mat& image) -{ - boost::unique_lock lock(mutex_); - image_ = image; - condition_.notify_one(); -} - -cv::Mat ThreadSafeImage::get() -{ - boost::unique_lock lock(mutex_); - return image_; -} - -cv::Mat ThreadSafeImage::pop() -{ - cv::Mat image; - { - boost::unique_lock lock(mutex_); - while (image_.empty()) - { - condition_.wait(lock); - } - image = image_; - image_.release(); - } - return image; -} - -class ImageNodelet : public nodelet::Nodelet -{ - image_transport::Subscriber sub_; - - boost::thread window_thread_; - - ThreadSafeImage queued_image_, shown_image_; - - std::string window_name_; - bool autosize_; - boost::format filename_format_; - int count_; - - ros::WallTimer gui_timer_; - - virtual void onInit(); - - void imageCb(const sensor_msgs::ImageConstPtr& msg); - - static void mouseCb(int event, int x, int y, int flags, void* param); - static void guiCb(const ros::WallTimerEvent&); - - void windowThread(); - -public: - ImageNodelet(); - - ~ImageNodelet(); -}; - -ImageNodelet::ImageNodelet() - : filename_format_(""), count_(0) -{ -} - -ImageNodelet::~ImageNodelet() -{ - if (window_thread_.joinable()) - { - window_thread_.interrupt(); - window_thread_.join(); - } -} - -void ImageNodelet::onInit() -{ - ros::NodeHandle nh = getNodeHandle(); - ros::NodeHandle local_nh = getPrivateNodeHandle(); - - // Command line argument parsing - const std::vector& argv = getMyArgv(); - // First positional argument is the transport type - std::string transport; - local_nh.param("image_transport", transport, std::string("raw")); - for (int i = 0; i < (int)argv.size(); ++i) - { - if (argv[i][0] != '-') - { - transport = argv[i]; - break; - } - } - NODELET_INFO_STREAM("Using transport \"" << transport << "\""); - // Internal option, should be used only by the image_view node - bool shutdown_on_close = std::find(argv.begin(), argv.end(), - "--shutdown-on-close") != argv.end(); - - // Default window name is the resolved topic name - std::string topic = nh.resolveName("image"); - local_nh.param("window_name", window_name_, topic); - - local_nh.param("autosize", autosize_, false); - - std::string format_string; - local_nh.param("filename_format", format_string, std::string("frame%04i.jpg")); - filename_format_.parse(format_string); - - // Since cv::startWindowThread() triggers a crash in cv::waitKey() - // if OpenCV is compiled against GTK, we call cv::waitKey() from - // the ROS event loop periodically, instead. - /*cv::startWindowThread();*/ - gui_timer_ = local_nh.createWallTimer(ros::WallDuration(0.1), ImageNodelet::guiCb); - - window_thread_ = boost::thread(&ImageNodelet::windowThread, this); - - image_transport::ImageTransport it(nh); - image_transport::TransportHints hints(transport, ros::TransportHints(), getPrivateNodeHandle()); - sub_ = it.subscribe(topic, 1, &ImageNodelet::imageCb, this, hints); -} - -void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg) -{ - // We want to scale floating point images so that they display nicely - bool do_dynamic_scaling = (msg->encoding.find("F") != std::string::npos); - - // Convert to OpenCV native BGR color - try { - cv_bridge::CvtColorForDisplayOptions options; - options.do_dynamic_scaling = do_dynamic_scaling; - queued_image_.set(cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options)->image); - } - catch (cv_bridge::Exception& e) { - NODELET_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'", - msg->encoding.c_str(), e.what()); - } -} - -void ImageNodelet::guiCb(const ros::WallTimerEvent&) -{ - // Process pending GUI events and return immediately - cv::waitKey(1); -} - -void ImageNodelet::mouseCb(int event, int x, int y, int flags, void* param) -{ - ImageNodelet *this_ = reinterpret_cast(param); - // Trick to use NODELET_* logging macros in static function - boost::function getName = - boost::bind(&ImageNodelet::getName, this_); - - if (event == cv::EVENT_LBUTTONDOWN) - { - NODELET_WARN_ONCE("Left-clicking no longer saves images. Right-click instead."); - return; - } - if (event != cv::EVENT_RBUTTONDOWN) - return; - - cv::Mat image(this_->shown_image_.get()); - if (image.empty()) - { - NODELET_WARN("Couldn't save image, no data!"); - return; - } - - std::string filename = (this_->filename_format_ % this_->count_).str(); - if (cv::imwrite(filename, image)) - { - NODELET_INFO("Saved image %s", filename.c_str()); - this_->count_++; - } - else - { - /// @todo Show full path, ask if user has permission to write there - NODELET_ERROR("Failed to save image."); - } -} - -void ImageNodelet::windowThread() -{ - cv::namedWindow(window_name_, autosize_ ? cv::WND_PROP_AUTOSIZE : 0); - cv::setMouseCallback(window_name_, &ImageNodelet::mouseCb, this); - - try - { - while (true) - { - cv::Mat image(queued_image_.pop()); - cv::imshow(window_name_, image); - cv::waitKey(1); - shown_image_.set(image); - } - } - catch (const boost::thread_interrupted&) - { - } - - cv::destroyWindow(window_name_); -} - -} // namespace image_view - -// Register the nodelet -#include -PLUGINLIB_EXPORT_CLASS( image_view::ImageNodelet, nodelet::Nodelet) diff --git a/image_view/src/nodes/extract_images.cpp b/image_view/src/nodes/extract_images.cpp deleted file mode 100644 index 9ebdd483a..000000000 --- a/image_view/src/nodes/extract_images.cpp +++ /dev/null @@ -1,154 +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. -*********************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -class ExtractImages -{ -private: - image_transport::Subscriber sub_; - - sensor_msgs::ImageConstPtr last_msg_; - boost::mutex image_mutex_; - - std::string window_name_; - boost::format filename_format_; - int count_; - double _time; - double sec_per_frame_; - -#if defined(_VIDEO) - CvVideoWriter* video_writer; -#endif //_VIDEO - -public: - ExtractImages(const ros::NodeHandle& nh, const std::string& transport) - : filename_format_(""), count_(0), _time(ros::Time::now().toSec()) - { - std::string topic = nh.resolveName("image"); - ros::NodeHandle local_nh("~"); - - std::string format_string; - local_nh.param("filename_format", format_string, std::string("frame%04i.jpg")); - filename_format_.parse(format_string); - - local_nh.param("sec_per_frame", sec_per_frame_, 0.1); - - image_transport::ImageTransport it(nh); - sub_ = it.subscribe(topic, 1, &ExtractImages::image_cb, this, transport); - -#if defined(_VIDEO) - video_writer = 0; -#endif - - ROS_INFO("Initialized sec per frame to %f", sec_per_frame_); - } - - ~ExtractImages() - { - } - - void image_cb(const sensor_msgs::ImageConstPtr& msg) - { - boost::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) - boost::const_pointer_cast(msg)->encoding = "mono8"; - - cv::Mat image; - try - { - image = cv_bridge::toCvShare(msg, "bgr8")->image; - } catch(cv_bridge::Exception) - { - ROS_ERROR("Unable to convert %s image to bgr8", msg->encoding.c_str()); - } - - double delay = ros::Time::now().toSec()-_time; - if(delay >= sec_per_frame_) - { - _time = ros::Time::now().toSec(); - - if (!image.empty()) { - std::string filename = (filename_format_ % count_).str(); - -#if !defined(_VIDEO) - cv::imwrite(filename, image); -#else - if(!video_writer) - { - video_writer = cvCreateVideoWriter("video.avi", CV_FOURCC('M','J','P','G'), - int(1.0/sec_per_frame_), cvSize(image->width, image->height)); - } - - cvWriteFrame(video_writer, image); -#endif // _VIDEO - - ROS_INFO("Saved image %s", filename.c_str()); - count_++; - } else { - ROS_WARN("Couldn't save image, no data!"); - } - } - } -}; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "extract_images", ros::init_options::AnonymousName); - ros::NodeHandle n; - if (n.resolveName("image") == "/image") { - ROS_WARN("extract_images: image has not been remapped! Typical command-line usage:\n" - "\t$ ./extract_images image:= [transport]"); - } - - ExtractImages view(n, (argc > 1) ? argv[1] : "raw"); - - ros::spin(); - - return 0; -} diff --git a/image_view/src/nodes/image_saver.cpp b/image_view/src/nodes/image_saver.cpp deleted file mode 100644 index d3509c31c..000000000 --- a/image_view/src/nodes/image_saver.cpp +++ /dev/null @@ -1,226 +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. -*********************************************************************/ -#include - -#include -#include -#include -#include -#include - -#include -#include - -boost::format g_format; -bool save_all_image, save_image_service; -std::string encoding; -bool request_start_end; - - -bool service(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { - save_image_service = true; - return true; -} - -/** Class to deal with which callback to call whether we have CameraInfo or not - */ -class Callbacks { -public: - Callbacks() : is_first_image_(true), has_camera_info_(false), count_(0) { - } - - bool callbackStartSave(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res) - { - ROS_INFO("Received start saving request"); - start_time_ = ros::Time::now(); - end_time_ = ros::Time(0); - - res.success = true; - return true; - } - - bool callbackEndSave(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res) - { - ROS_INFO("Received end saving request"); - end_time_ = ros::Time::now(); - - res.success = true; - return true; - } - - void callbackWithoutCameraInfo(const sensor_msgs::ImageConstPtr& image_msg) - { - if (is_first_image_) { - is_first_image_ = false; - - // Wait a tiny bit to see whether callbackWithCameraInfo is called - ros::Duration(0.001).sleep(); - } - - if (has_camera_info_) - return; - - // saving flag priority: - // 1. request by service. - // 2. request by topic about start and end. - // 3. flag 'save_all_image'. - if (!save_image_service && request_start_end) { - if (start_time_ == ros::Time(0)) - return; - else if (start_time_ > image_msg->header.stamp) - return; // wait for message which comes after start_time - else if ((end_time_ != ros::Time(0)) && (end_time_ < image_msg->header.stamp)) - return; // skip message which comes after end_time - } - - // save the image - std::string filename; - if (!saveImage(image_msg, filename)) - return; - - count_++; - } - - void callbackWithCameraInfo(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info) - { - has_camera_info_ = true; - - if (!save_image_service && request_start_end) { - if (start_time_ == ros::Time(0)) - return; - else if (start_time_ > image_msg->header.stamp) - return; // wait for message which comes after start_time - else if ((end_time_ != ros::Time(0)) && (end_time_ < image_msg->header.stamp)) - return; // skip message which comes after end_time - } - - // 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); - } - - count_++; - } -private: - bool saveImage(const sensor_msgs::ImageConstPtr& image_msg, std::string &filename) { - cv::Mat image; - try - { - image = cv_bridge::toCvShare(image_msg, encoding)->image; - } catch(cv_bridge::Exception) - { - ROS_ERROR("Unable to convert %s image to %s", image_msg->encoding.c_str(), encoding.c_str()); - return false; - } - - if (!image.empty()) { - try { - filename = (g_format).str(); - } catch (...) { g_format.clear(); } - try { - filename = (g_format % count_).str(); - } catch (...) { g_format.clear(); } - try { - filename = (g_format % count_ % "jpg").str(); - } catch (...) { g_format.clear(); } - - if ( save_all_image || save_image_service ) { - cv::imwrite(filename, image); - ROS_INFO("Saved image %s", filename.c_str()); - - save_image_service = false; - } else { - return false; - } - } else { - ROS_WARN("Couldn't save image, no data!"); - return false; - } - return true; - } - -private: - bool is_first_image_; - bool has_camera_info_; - size_t count_; - ros::Time start_time_; - ros::Time end_time_; -}; - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "image_saver", ros::init_options::AnonymousName); - ros::NodeHandle nh; - image_transport::ImageTransport it(nh); - std::string topic = nh.resolveName("image"); - - Callbacks callbacks; - // Useful when CameraInfo is being published - image_transport::CameraSubscriber sub_image_and_camera = it.subscribeCamera(topic, 1, - &Callbacks::callbackWithCameraInfo, - &callbacks); - // Useful when CameraInfo is not being published - image_transport::Subscriber sub_image = it.subscribe( - topic, 1, boost::bind(&Callbacks::callbackWithoutCameraInfo, &callbacks, _1)); - - ros::NodeHandle local_nh("~"); - std::string format_string; - local_nh.param("filename_format", format_string, std::string("left%04i.%s")); - local_nh.param("encoding", encoding, std::string("bgr8")); - local_nh.param("save_all_image", save_all_image, true); - local_nh.param("request_start_end", request_start_end, false); - g_format.parse(format_string); - ros::ServiceServer save = local_nh.advertiseService ("save", service); - - if (request_start_end && !save_all_image) - ROS_WARN("'request_start_end' is true, so overwriting 'save_all_image' as true"); - - // FIXME(unkown): This does not make services appear - // if (request_start_end) { - ros::ServiceServer srv_start = local_nh.advertiseService( - "start", &Callbacks::callbackStartSave, &callbacks); - ros::ServiceServer srv_end = local_nh.advertiseService( - "end", &Callbacks::callbackEndSave, &callbacks); - // } - - ros::spin(); -} diff --git a/image_view/src/nodes/image_view.cpp b/image_view/src/nodes/image_view.cpp deleted file mode 100644 index df7f9933b..000000000 --- a/image_view/src/nodes/image_view.cpp +++ /dev/null @@ -1,227 +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. -*********************************************************************/ -#include - -#include -#include -#include - -#include -#include - -#include -#include -#include - -int g_count; -cv::Mat g_last_image; -boost::format g_filename_format; -boost::mutex g_image_mutex; -std::string g_window_name; -bool g_gui; -ros::Publisher g_pub; -bool g_do_dynamic_scaling; -int g_colormap; -double g_min_image_value; -double g_max_image_value; - -void reconfigureCb(image_view::ImageViewConfig &config, uint32_t level) -{ - boost::mutex::scoped_lock lock(g_image_mutex); - g_do_dynamic_scaling = config.do_dynamic_scaling; - g_colormap = config.colormap; - g_min_image_value = config.min_image_value; - g_max_image_value = config.max_image_value; -} - -void imageCb(const sensor_msgs::ImageConstPtr& msg) -{ - boost::mutex::scoped_lock lock(g_image_mutex); - - // Convert to OpenCV native BGR color - cv_bridge::CvImageConstPtr cv_ptr; - try { - cv_bridge::CvtColorForDisplayOptions options; - options.do_dynamic_scaling = g_do_dynamic_scaling; - options.colormap = g_colormap; - // Set min/max value for scaling to visualize depth/float image. - if (g_min_image_value == g_max_image_value) { - // Not specified by rosparam, then set default value. - // Because of current sensor limitation, we use 10m as default of max range of depth - // with consistency to the configuration in rqt_image_view. - options.min_image_value = 0; - if (msg->encoding == "32FC1") { - options.max_image_value = 10; // 10 [m] - } else if (msg->encoding == "16UC1") { - options.max_image_value = 10 * 1000; // 10 * 1000 [mm] - } - } else { - options.min_image_value = g_min_image_value; - options.max_image_value = g_max_image_value; - } - cv_ptr = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options); - g_last_image = cv_ptr->image; - } catch (cv_bridge::Exception& e) { - ROS_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'", - msg->encoding.c_str(), e.what()); - } - if (g_gui && !g_last_image.empty()) { - const cv::Mat &image = g_last_image; - cv::imshow(g_window_name, image); - cv::waitKey(1); - } - if (g_pub.getNumSubscribers() > 0) { - g_pub.publish(cv_ptr); - } -} - -static void mouseCb(int event, int x, int y, int flags, void* param) -{ - if (event == cv::EVENT_LBUTTONDOWN) { - ROS_WARN_ONCE("Left-clicking no longer saves images. Right-click instead."); - return; - } else if (event != cv::EVENT_RBUTTONDOWN) { - return; - } - - boost::mutex::scoped_lock lock(g_image_mutex); - - const cv::Mat &image = g_last_image; - - if (image.empty()) { - ROS_WARN("Couldn't save image, no data!"); - return; - } - - std::string filename = (g_filename_format % g_count).str(); - if (cv::imwrite(filename, image)) { - ROS_INFO("Saved image %s", filename.c_str()); - g_count++; - } else { - boost::filesystem::path full_path = boost::filesystem::complete(filename); - ROS_ERROR_STREAM("Failed to save image. Have permission to write there?: " << full_path); - } -} - -static void guiCb(const ros::WallTimerEvent&) -{ - // Process pending GUI events and return immediately - cv::waitKey(1); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "image_view", ros::init_options::AnonymousName); - if (ros::names::remap("image") == "image") { - ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n" - "\t$ rosrun image_view image_view image:= [transport]"); - } - - ros::NodeHandle nh; - ros::NodeHandle local_nh("~"); - ros::WallTimer gui_timer; - - // Default window name is the resolved topic name - std::string topic = nh.resolveName("image"); - local_nh.param("window_name", g_window_name, topic); - local_nh.param("gui", g_gui, true); // gui/no_gui mode - - if (g_gui) { - std::string format_string; - local_nh.param("filename_format", format_string, std::string("frame%04i.jpg")); - g_filename_format.parse(format_string); - - // Handle window size - bool autosize; - local_nh.param("autosize", autosize, false); - cv::namedWindow(g_window_name, autosize ? (CV_WINDOW_AUTOSIZE | CV_WINDOW_KEEPRATIO | CV_GUI_EXPANDED) : 0); - cv::setMouseCallback(g_window_name, &mouseCb); - - if(autosize == false) - { - if(local_nh.hasParam("width") && local_nh.hasParam("height")) - { - int width; - local_nh.getParam("width", width); - int height; - local_nh.getParam("height", height); - cv::resizeWindow(g_window_name, width, height); - } - } - - // Since cv::startWindowThread() triggers a crash in cv::waitKey() - // if OpenCV is compiled against GTK, we call cv::waitKey() from - // the ROS event loop periodically, instead. - /*cv::startWindowThread();*/ - gui_timer = local_nh.createWallTimer(ros::WallDuration(0.1), guiCb); - } - - // Handle transport - // priority: - // 1. command line argument - // 2. rosparam '~image_transport' - std::string transport; - local_nh.param("image_transport", transport, std::string("raw")); - ros::V_string myargv; - ros::removeROSArgs(argc, argv, myargv); - for (size_t i = 1; i < myargv.size(); ++i) { - if (myargv[i][0] != '-') { - transport = myargv[i]; - break; - } - } - ROS_INFO_STREAM("Using transport \"" << transport << "\""); - image_transport::ImageTransport it(nh); - image_transport::TransportHints hints(transport, ros::TransportHints(), local_nh); - image_transport::Subscriber sub = it.subscribe(topic, 1, imageCb, hints); - g_pub = local_nh.advertise("output", 1); - - dynamic_reconfigure::Server srv; - dynamic_reconfigure::Server::CallbackType f = - boost::bind(&reconfigureCb, _1, _2); - srv.setCallback(f); - - ros::spin(); - - if (g_gui) { - cv::destroyWindow(g_window_name); - } - // The publisher is a global variable, and therefore its scope exceeds those - // of the node handles in main(). Unfortunately, this will cause a crash - // when the publisher tries to shut down and all node handles are gone - // already. Therefore, we shut down the publisher now and avoid the annoying - // mutex assertion. - g_pub.shutdown(); - return 0; -} diff --git a/image_view/src/nodes/stereo_view.cpp b/image_view/src/nodes/stereo_view.cpp deleted file mode 100644 index 64a01ccb3..000000000 --- a/image_view/src/nodes/stereo_view.cpp +++ /dev/null @@ -1,573 +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. -*********************************************************************/ - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#ifdef HAVE_GTK -#include - -// Platform-specific workaround for #3026: image_view doesn't close when -// closing image window. On platforms using GTK+ we connect this to the -// window's "destroy" event so that image_view exits. -static void destroy(GtkWidget *widget, gpointer data) -{ - ros::shutdown(); -} -#endif - -namespace enc = sensor_msgs::image_encodings; - -// colormap for disparities, RGB -static unsigned char colormap[768] = - { 150, 150, 150, - 107, 0, 12, - 106, 0, 18, - 105, 0, 24, - 103, 0, 30, - 102, 0, 36, - 101, 0, 42, - 99, 0, 48, - 98, 0, 54, - 97, 0, 60, - 96, 0, 66, - 94, 0, 72, - 93, 0, 78, - 92, 0, 84, - 91, 0, 90, - 89, 0, 96, - 88, 0, 102, - 87, 0, 108, - 85, 0, 114, - 84, 0, 120, - 83, 0, 126, - 82, 0, 131, - 80, 0, 137, - 79, 0, 143, - 78, 0, 149, - 77, 0, 155, - 75, 0, 161, - 74, 0, 167, - 73, 0, 173, - 71, 0, 179, - 70, 0, 185, - 69, 0, 191, - 68, 0, 197, - 66, 0, 203, - 65, 0, 209, - 64, 0, 215, - 62, 0, 221, - 61, 0, 227, - 60, 0, 233, - 59, 0, 239, - 57, 0, 245, - 56, 0, 251, - 55, 0, 255, - 54, 0, 255, - 52, 0, 255, - 51, 0, 255, - 50, 0, 255, - 48, 0, 255, - 47, 0, 255, - 46, 0, 255, - 45, 0, 255, - 43, 0, 255, - 42, 0, 255, - 41, 0, 255, - 40, 0, 255, - 38, 0, 255, - 37, 0, 255, - 36, 0, 255, - 34, 0, 255, - 33, 0, 255, - 32, 0, 255, - 31, 0, 255, - 29, 0, 255, - 28, 0, 255, - 27, 0, 255, - 26, 0, 255, - 24, 0, 255, - 23, 0, 255, - 22, 0, 255, - 20, 0, 255, - 19, 0, 255, - 18, 0, 255, - 17, 0, 255, - 15, 0, 255, - 14, 0, 255, - 13, 0, 255, - 11, 0, 255, - 10, 0, 255, - 9, 0, 255, - 8, 0, 255, - 6, 0, 255, - 5, 0, 255, - 4, 0, 255, - 3, 0, 255, - 1, 0, 255, - 0, 4, 255, - 0, 10, 255, - 0, 16, 255, - 0, 22, 255, - 0, 28, 255, - 0, 34, 255, - 0, 40, 255, - 0, 46, 255, - 0, 52, 255, - 0, 58, 255, - 0, 64, 255, - 0, 70, 255, - 0, 76, 255, - 0, 82, 255, - 0, 88, 255, - 0, 94, 255, - 0, 100, 255, - 0, 106, 255, - 0, 112, 255, - 0, 118, 255, - 0, 124, 255, - 0, 129, 255, - 0, 135, 255, - 0, 141, 255, - 0, 147, 255, - 0, 153, 255, - 0, 159, 255, - 0, 165, 255, - 0, 171, 255, - 0, 177, 255, - 0, 183, 255, - 0, 189, 255, - 0, 195, 255, - 0, 201, 255, - 0, 207, 255, - 0, 213, 255, - 0, 219, 255, - 0, 225, 255, - 0, 231, 255, - 0, 237, 255, - 0, 243, 255, - 0, 249, 255, - 0, 255, 255, - 0, 255, 249, - 0, 255, 243, - 0, 255, 237, - 0, 255, 231, - 0, 255, 225, - 0, 255, 219, - 0, 255, 213, - 0, 255, 207, - 0, 255, 201, - 0, 255, 195, - 0, 255, 189, - 0, 255, 183, - 0, 255, 177, - 0, 255, 171, - 0, 255, 165, - 0, 255, 159, - 0, 255, 153, - 0, 255, 147, - 0, 255, 141, - 0, 255, 135, - 0, 255, 129, - 0, 255, 124, - 0, 255, 118, - 0, 255, 112, - 0, 255, 106, - 0, 255, 100, - 0, 255, 94, - 0, 255, 88, - 0, 255, 82, - 0, 255, 76, - 0, 255, 70, - 0, 255, 64, - 0, 255, 58, - 0, 255, 52, - 0, 255, 46, - 0, 255, 40, - 0, 255, 34, - 0, 255, 28, - 0, 255, 22, - 0, 255, 16, - 0, 255, 10, - 0, 255, 4, - 2, 255, 0, - 8, 255, 0, - 14, 255, 0, - 20, 255, 0, - 26, 255, 0, - 32, 255, 0, - 38, 255, 0, - 44, 255, 0, - 50, 255, 0, - 56, 255, 0, - 62, 255, 0, - 68, 255, 0, - 74, 255, 0, - 80, 255, 0, - 86, 255, 0, - 92, 255, 0, - 98, 255, 0, - 104, 255, 0, - 110, 255, 0, - 116, 255, 0, - 122, 255, 0, - 128, 255, 0, - 133, 255, 0, - 139, 255, 0, - 145, 255, 0, - 151, 255, 0, - 157, 255, 0, - 163, 255, 0, - 169, 255, 0, - 175, 255, 0, - 181, 255, 0, - 187, 255, 0, - 193, 255, 0, - 199, 255, 0, - 205, 255, 0, - 211, 255, 0, - 217, 255, 0, - 223, 255, 0, - 229, 255, 0, - 235, 255, 0, - 241, 255, 0, - 247, 255, 0, - 253, 255, 0, - 255, 251, 0, - 255, 245, 0, - 255, 239, 0, - 255, 233, 0, - 255, 227, 0, - 255, 221, 0, - 255, 215, 0, - 255, 209, 0, - 255, 203, 0, - 255, 197, 0, - 255, 191, 0, - 255, 185, 0, - 255, 179, 0, - 255, 173, 0, - 255, 167, 0, - 255, 161, 0, - 255, 155, 0, - 255, 149, 0, - 255, 143, 0, - 255, 137, 0, - 255, 131, 0, - 255, 126, 0, - 255, 120, 0, - 255, 114, 0, - 255, 108, 0, - 255, 102, 0, - 255, 96, 0, - 255, 90, 0, - 255, 84, 0, - 255, 78, 0, - 255, 72, 0, - 255, 66, 0, - 255, 60, 0, - 255, 54, 0, - 255, 48, 0, - 255, 42, 0, - 255, 36, 0, - 255, 30, 0, - 255, 24, 0, - 255, 18, 0, - 255, 12, 0, - 255, 6, 0, - 255, 0, 0, - }; - -inline void increment(int* value) -{ - ++(*value); -} - -using namespace sensor_msgs; -using namespace stereo_msgs; -using namespace message_filters::sync_policies; - -// Note: StereoView is NOT nodelet-based, as it synchronizes the three streams. -class StereoView -{ -private: - image_transport::SubscriberFilter left_sub_, right_sub_; - message_filters::Subscriber disparity_sub_; - typedef ExactTime ExactPolicy; - typedef ApproximateTime ApproximatePolicy; - typedef message_filters::Synchronizer ExactSync; - typedef message_filters::Synchronizer ApproximateSync; - boost::shared_ptr exact_sync_; - boost::shared_ptr approximate_sync_; - int queue_size_; - - ImageConstPtr last_left_msg_, last_right_msg_; - cv::Mat last_left_image_, last_right_image_; - cv::Mat_ disparity_color_; - boost::mutex image_mutex_; - - boost::format filename_format_; - int save_count_; - - ros::WallTimer check_synced_timer_; - int left_received_, right_received_, disp_received_, all_received_; - -public: - StereoView(const std::string& transport) - : filename_format_(""), save_count_(0), - left_received_(0), right_received_(0), disp_received_(0), all_received_(0) - { - // Read local parameters - ros::NodeHandle local_nh("~"); - bool autosize; - local_nh.param("autosize", autosize, true); - - std::string format_string; - local_nh.param("filename_format", format_string, std::string("%s%04i.jpg")); - filename_format_.parse(format_string); - - // Do GUI window setup - int flags = autosize ? cv::WND_PROP_AUTOSIZE : 0; - cv::namedWindow("left", flags); - cv::namedWindow("right", flags); - cv::namedWindow("disparity", flags); - cv::setMouseCallback("left", &StereoView::mouseCb, this); - cv::setMouseCallback("right", &StereoView::mouseCb, this); - cv::setMouseCallback("disparity", &StereoView::mouseCb, this); -#if CV_MAJOR_VERSION == 2 -#ifdef HAVE_GTK - g_signal_connect(GTK_WIDGET( cvGetWindowHandle("left") ), - "destroy", G_CALLBACK(destroy), NULL); - g_signal_connect(GTK_WIDGET( cvGetWindowHandle("right") ), - "destroy", G_CALLBACK(destroy), NULL); - g_signal_connect(GTK_WIDGET( cvGetWindowHandle("disparity") ), - "destroy", G_CALLBACK(destroy), NULL); -#endif - cvStartWindowThread(); -#endif - - // Resolve topic names - ros::NodeHandle nh; - std::string stereo_ns = nh.resolveName("stereo"); - std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image")); - std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image")); - std::string disparity_topic = ros::names::clean(stereo_ns + "/disparity"); - ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s", left_topic.c_str(), right_topic.c_str(), - disparity_topic.c_str()); - - // Subscribe to three input topics. - image_transport::ImageTransport it(nh); - left_sub_.subscribe(it, left_topic, 1, transport); - right_sub_.subscribe(it, right_topic, 1, transport); - disparity_sub_.subscribe(nh, disparity_topic, 1); - - // Complain every 30s if the topics appear unsynchronized - left_sub_.registerCallback(boost::bind(increment, &left_received_)); - right_sub_.registerCallback(boost::bind(increment, &right_received_)); - disparity_sub_.registerCallback(boost::bind(increment, &disp_received_)); - check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0), - boost::bind(&StereoView::checkInputsSynchronized, this)); - - // Synchronize input topics. Optionally do approximate synchronization. - local_nh.param("queue_size", queue_size_, 5); - bool approx; - local_nh.param("approximate_sync", approx, false); - if (approx) - { - approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size_), - left_sub_, right_sub_, disparity_sub_) ); - approximate_sync_->registerCallback(boost::bind(&StereoView::imageCb, this, _1, _2, _3)); - } - else - { - exact_sync_.reset( new ExactSync(ExactPolicy(queue_size_), - left_sub_, right_sub_, disparity_sub_) ); - exact_sync_->registerCallback(boost::bind(&StereoView::imageCb, this, _1, _2, _3)); - } - } - - ~StereoView() - { - cv::destroyAllWindows(); - } - - void imageCb(const ImageConstPtr& left, const ImageConstPtr& right, - const DisparityImageConstPtr& disparity_msg) - { - ++all_received_; // For error checking - - image_mutex_.lock(); - - // May want to view raw bayer data - if (left->encoding.find("bayer") != std::string::npos) - boost::const_pointer_cast(left)->encoding = "mono8"; - if (right->encoding.find("bayer") != std::string::npos) - boost::const_pointer_cast(right)->encoding = "mono8"; - - // Hang on to image data for sake of mouseCb - last_left_msg_ = left; - last_right_msg_ = right; - try { - last_left_image_ = cv_bridge::toCvShare(left, "bgr8")->image; - last_right_image_ = cv_bridge::toCvShare(right, "bgr8")->image; - } - catch (cv_bridge::Exception& e) { - ROS_ERROR("Unable to convert one of '%s' or '%s' to 'bgr8'", - left->encoding.c_str(), right->encoding.c_str()); - } - - // Colormap and display the disparity image - float min_disparity = disparity_msg->min_disparity; - float max_disparity = disparity_msg->max_disparity; - float multiplier = 255.0f / (max_disparity - min_disparity); - - assert(disparity_msg->image.encoding == enc::TYPE_32FC1); - const cv::Mat_ dmat(disparity_msg->image.height, disparity_msg->image.width, - (float*)&disparity_msg->image.data[0], disparity_msg->image.step); - disparity_color_.create(disparity_msg->image.height, disparity_msg->image.width); - - for (int row = 0; row < disparity_color_.rows; ++row) { - const float* d = dmat[row]; - for (int col = 0; col < disparity_color_.cols; ++col) { - int index = (d[col] - min_disparity) * multiplier + 0.5; - index = std::min(255, std::max(0, index)); - // Fill as BGR - disparity_color_(row, col)[2] = colormap[3*index + 0]; - disparity_color_(row, col)[1] = colormap[3*index + 1]; - disparity_color_(row, col)[0] = colormap[3*index + 2]; - } - } - - // Must release the mutex before calling cv::imshow, or can deadlock against - // OpenCV's window mutex. - image_mutex_.unlock(); - if (!last_left_image_.empty()) { - cv::imshow("left", last_left_image_); - cv::waitKey(1); - } - if (!last_right_image_.empty()) { - cv::imshow("right", last_right_image_); - cv::waitKey(1); - } - cv::imshow("disparity", disparity_color_); - cv::waitKey(1); - } - - void saveImage(const char* prefix, const cv::Mat& image) - { - if (!image.empty()) { - std::string filename = (filename_format_ % prefix % save_count_).str(); - cv::imwrite(filename, image); - ROS_INFO("Saved image %s", filename.c_str()); - } else { - ROS_WARN("Couldn't save %s image, no data!", prefix); - } - } - - static void mouseCb(int event, int x, int y, int flags, void* param) - { - if (event == cv::EVENT_LBUTTONDOWN) - { - ROS_WARN_ONCE("Left-clicking no longer saves images. Right-click instead."); - return; - } - if (event != cv::EVENT_RBUTTONDOWN) - return; - - StereoView *sv = (StereoView*)param; - boost::lock_guard guard(sv->image_mutex_); - - sv->saveImage("left", sv->last_left_image_); - sv->saveImage("right", sv->last_right_image_); - sv->saveImage("disp", sv->disparity_color_); - sv->save_count_++; - } - - void checkInputsSynchronized() - { - int threshold = 3 * all_received_; - if (left_received_ >= threshold || right_received_ >= threshold || disp_received_ >= threshold) { - ROS_WARN("[stereo_view] Low number of synchronized left/right/disparity triplets received.\n" - "Left images received: %d (topic '%s')\n" - "Right images received: %d (topic '%s')\n" - "Disparity images received: %d (topic '%s')\n" - "Synchronized triplets: %d\n" - "Possible issues:\n" - "\t* stereo_image_proc is not running.\n" - "\t Does `rosnode info %s` show any connections?\n" - "\t* The cameras are not synchronized.\n" - "\t Try restarting stereo_view with parameter _approximate_sync:=True\n" - "\t* The network is too slow. One or more images are dropped from each triplet.\n" - "\t Try restarting stereo_view, increasing parameter 'queue_size' (currently %d)", - left_received_, left_sub_.getTopic().c_str(), - right_received_, right_sub_.getTopic().c_str(), - disp_received_, disparity_sub_.getTopic().c_str(), - all_received_, ros::this_node::getName().c_str(), queue_size_); - } - } -}; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "stereo_view", ros::init_options::AnonymousName); - if (ros::names::remap("stereo") == "stereo") { - ROS_WARN("'stereo' has not been remapped! Example command-line usage:\n" - "\t$ rosrun image_view stereo_view stereo:=narrow_stereo image:=image_color"); - } - if (ros::names::remap("image") == "/image_raw") { - ROS_WARN("There is a delay between when the camera drivers publish the raw images and " - "when stereo_image_proc publishes the computed point cloud. stereo_view " - "may fail to synchronize these topics without a large queue_size."); - } - - std::string transport = argc > 1 ? argv[1] : "raw"; - StereoView view(transport); - - ros::spin(); - return 0; -} diff --git a/image_view/src/nodes/video_recorder.cpp b/image_view/src/nodes/video_recorder.cpp deleted file mode 100644 index 56b7a0245..000000000 --- a/image_view/src/nodes/video_recorder.cpp +++ /dev/null @@ -1,141 +0,0 @@ -/**************************************************************************** -* Software License Agreement (Apache License) -* -* Copyright (C) 2012-2013 Open Source Robotics Foundation -* -* 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 -#include -#include -#if CV_MAJOR_VERSION == 3 -#include -#endif - -cv::VideoWriter outputVideo; - -int g_count = 0; -ros::Time g_last_wrote_time = ros::Time(0); -std::string encoding; -std::string codec; -int fps; -std::string filename; -double min_depth_range; -double max_depth_range; -bool use_dynamic_range; -int colormap; - - -void callback(const sensor_msgs::ImageConstPtr& image_msg) -{ - if (!outputVideo.isOpened()) { - - cv::Size size(image_msg->width, image_msg->height); - - outputVideo.open(filename, -#if CV_MAJOR_VERSION == 3 - cv::VideoWriter::fourcc(codec.c_str()[0], -#else - CV_FOURCC(codec.c_str()[0], -#endif - codec.c_str()[1], - codec.c_str()[2], - codec.c_str()[3]), - fps, - size, - true); - - if (!outputVideo.isOpened()) - { - ROS_ERROR("Could not create the output video! Check filename and/or support for codec."); - exit(-1); - } - - ROS_INFO_STREAM("Starting to record " << codec << " video at " << size << "@" << fps << "fps. Press Ctrl+C to stop recording." ); - - } - - if ((image_msg->header.stamp - g_last_wrote_time) < ros::Duration(1.0 / fps)) - { - // Skip to get video with correct fps - return; - } - - try - { - cv_bridge::CvtColorForDisplayOptions options; - options.do_dynamic_scaling = use_dynamic_range; - options.min_image_value = min_depth_range; - options.max_image_value = max_depth_range; - options.colormap = colormap; - const cv::Mat image = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg), encoding, options)->image; - if (!image.empty()) { - outputVideo << image; - ROS_INFO_STREAM("Recording frame " << g_count << "\x1b[1F"); - g_count++; - g_last_wrote_time = image_msg->header.stamp; - } else { - ROS_WARN("Frame skipped, no data!"); - } - } catch(cv_bridge::Exception) - { - ROS_ERROR("Unable to convert %s image to %s", image_msg->encoding.c_str(), encoding.c_str()); - return; - } -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "video_recorder", ros::init_options::AnonymousName); - ros::NodeHandle nh; - ros::NodeHandle local_nh("~"); - local_nh.param("filename", filename, std::string("output.avi")); - bool stamped_filename; - local_nh.param("stamped_filename", stamped_filename, false); - local_nh.param("fps", fps, 15); - local_nh.param("codec", codec, std::string("MJPG")); - local_nh.param("encoding", encoding, std::string("bgr8")); - // cv_bridge::CvtColorForDisplayOptions - local_nh.param("min_depth_range", min_depth_range, 0.0); - local_nh.param("max_depth_range", max_depth_range, 0.0); - local_nh.param("use_dynamic_depth_range", use_dynamic_range, false); - local_nh.param("colormap", colormap, -1); - - if (stamped_filename) { - std::size_t found = filename.find_last_of("/\\"); - std::string path = filename.substr(0, found + 1); - std::string basename = filename.substr(found + 1); - std::stringstream ss; - ss << ros::Time::now().toNSec() << basename; - filename = path + ss.str(); - ROS_INFO("Video recording to %s", filename.c_str()); - } - - if (codec.size() != 4) { - ROS_ERROR("The video codec must be a FOURCC identifier (4 chars)"); - exit(-1); - } - - image_transport::ImageTransport it(nh); - std::string topic = nh.resolveName("image"); - image_transport::Subscriber sub_image = it.subscribe(topic, 1, callback); - - ROS_INFO_STREAM("Waiting for topic " << topic << "..."); - ros::spin(); - std::cout << "\nVideo saved as " << filename << std::endl; -} diff --git a/image_view/src/stereo_view.cpp b/image_view/src/stereo_view.cpp new file mode 100644 index 000000000..59436e6d7 --- /dev/null +++ b/image_view/src/stereo_view.cpp @@ -0,0 +1,68 @@ +/********************************************************************* +* 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 "image_view/stereo_view_node.hpp" + +int main(int argc, char ** argv) +{ + using image_view::StereoViewNode; + + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + options.append_parameter_override("transport", (argc > 1) ? argv[1] : "raw"); + auto svn = std::make_shared(options); + + rclcpp::spin(svn); + + return 0; +} diff --git a/image_view/src/stereo_view_node.cpp b/image_view/src/stereo_view_node.cpp new file mode 100644 index 000000000..2c92a3a63 --- /dev/null +++ b/image_view/src/stereo_view_node.cpp @@ -0,0 +1,309 @@ +/********************************************************************* +* 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 +#include +#include +#include +#include +#include +#include +#include + +#include + +#ifdef HAVE_GTK + +#include + +#endif + +#include +#include +#include + +#include "image_view/stereo_view_node.hpp" + +namespace image_view +{ + +namespace enc = sensor_msgs::image_encodings; + +using sensor_msgs::msg::Image; +using stereo_msgs::msg::DisparityImage; +using message_filters::sync_policies::ExactTime; +using message_filters::sync_policies::ApproximateTime; +using std::placeholders::_1; +using std::placeholders::_2; +using std::placeholders::_3; + +constexpr unsigned char StereoViewNode::colormap[768]; + +StereoViewNode::StereoViewNode(const rclcpp::NodeOptions & options) +: rclcpp::Node("stereo_view_node", options), + filename_format_(""), save_count_(0), + left_received_(0), right_received_(0), disp_received_(0), all_received_(0) +{ + // Read local parameters + bool autosize = this->declare_parameter("autosize", true); + + std::string format_string; + format_string = this->declare_parameter("filename_format", std::string("%s%04i.jpg")); + filename_format_.parse(format_string); + + std::string transport = this->declare_parameter("transport", std::string("raw")); + + // Do GUI window setup + int flags = autosize ? (cv::WINDOW_AUTOSIZE | cv::WINDOW_KEEPRATIO | cv::WINDOW_GUI_EXPANDED) : 0; + cv::namedWindow("left", flags); + cv::namedWindow("right", flags); + cv::namedWindow("disparity", flags); + cv::setMouseCallback("left", &StereoViewNode::mouseCb, this); + cv::setMouseCallback("right", &StereoViewNode::mouseCb, this); + cv::setMouseCallback("disparity", &StereoViewNode::mouseCb, this); + + // Resolve topic names + std::string stereo_ns = rclcpp::expand_topic_or_service_name( + "stereo", this->get_name(), this->get_namespace()); + std::string left_topic = rclcpp::expand_topic_or_service_name(stereo_ns + "/left/" + + rclcpp::expand_topic_or_service_name("image", this->get_name(), this->get_namespace()), + this->get_name(), this->get_namespace()); + std::string right_topic = rclcpp::expand_topic_or_service_name(stereo_ns + "/right/" + + rclcpp::expand_topic_or_service_name("image", this->get_name(), this->get_namespace()), + this->get_name(), this->get_namespace()); + std::string disparity_topic = rclcpp::expand_topic_or_service_name(stereo_ns + "/disparity", + this->get_name(), this->get_namespace()); + RCLCPP_INFO( + this->get_logger(), + "Subscribing to:\n\t* %s\n\t* %s\n\t* %s", + left_topic.c_str(), right_topic.c_str(), + disparity_topic.c_str()); + + // Subscribe to three input topics. + left_sub_.subscribe(this, left_topic, transport); + right_sub_.subscribe(this, right_topic, transport); + disparity_sub_.subscribe(this, disparity_topic); + + // Complain every 30s if the topics appear unsynchronized + left_sub_.registerCallback(std::bind(increment, &left_received_)); + right_sub_.registerCallback(std::bind(increment, &right_received_)); + disparity_sub_.registerCallback(std::bind(increment, &disp_received_)); + check_synced_timer_ = this->create_wall_timer( + std::chrono::seconds(15), + std::bind(&StereoViewNode::checkInputsSynchronized, this)); + + // Synchronize input topics. Optionally do approximate synchronization. + queue_size_ = this->declare_parameter("queue_size", 5); + bool approx = this->declare_parameter("approximate_sync", false); + if (approx) { + approximate_sync_.reset(new ApproximateSync( + ApproximatePolicy(queue_size_), left_sub_, right_sub_, disparity_sub_)); + approximate_sync_->registerCallback(std::bind( + &StereoViewNode::imageCb, this, _1, _2, _3)); + } else { + exact_sync_.reset( new ExactSync(ExactPolicy(queue_size_), + left_sub_, right_sub_, disparity_sub_) ); + exact_sync_->registerCallback(std::bind( + &StereoViewNode::imageCb, this, _1, _2, _3)); + } + + std::string stereo_topic = rclcpp::expand_topic_or_service_name("stereo", this->get_name(), this->get_namespace()); + std::string image_topic = rclcpp::expand_topic_or_service_name("image", this->get_name(), this->get_namespace()); + auto topics = this->get_topic_names_and_types(); + + if (topics.find(stereo_topic) != topics.end()) { + RCLCPP_WARN( + this->get_logger(), "'stereo' has not been remapped! Example command-line usage:\n" + "\t$ rosrun image_view stereo_view stereo:=narrow_stereo image:=image_color"); + } + if (topics.find(image_topic) != topics.end()) { + RCLCPP_WARN( + this->get_logger(), "There is a delay between when the camera drivers publish the raw images and " + "when stereo_image_proc publishes the computed point cloud. stereo_view " + "may fail to synchronize these topics without a large queue_size."); + } +} + +StereoViewNode::~StereoViewNode() +{ + cv::destroyAllWindows(); +} + +void StereoViewNode::imageCb( + const Image::ConstSharedPtr & left, const Image::ConstSharedPtr & right, + const DisparityImage::ConstSharedPtr& disparity_msg) +{ + ++all_received_; // For error checking + + image_mutex_.lock(); + + // May want to view raw bayer data + if (left->encoding.find("bayer") != std::string::npos) + std::const_pointer_cast(left)->encoding = "mono8"; + if (right->encoding.find("bayer") != std::string::npos) + std::const_pointer_cast(right)->encoding = "mono8"; + + // Hang on to image data for sake of mouseCb + last_left_msg_ = left; + last_right_msg_ = right; + + try { + last_left_image_ = cv_bridge::toCvShare(left, "bgr8")->image; + last_right_image_ = cv_bridge::toCvShare(right, "bgr8")->image; + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR( + this->get_logger(), "Unable to convert one of '%s' or '%s' to 'bgr8'", + left->encoding.c_str(), right->encoding.c_str()); + } + + // Colormap and display the disparity image + float min_disparity = disparity_msg->min_disparity; + float max_disparity = disparity_msg->max_disparity; + float multiplier = 255.0f / (max_disparity - min_disparity); + + assert(disparity_msg->image.encoding == enc::TYPE_32FC1); + const cv::Mat_ dmat(disparity_msg->image.height, disparity_msg->image.width, + (float*)&disparity_msg->image.data[0], disparity_msg->image.step); + disparity_color_.create(disparity_msg->image.height, disparity_msg->image.width); + + for (int row = 0; row < disparity_color_.rows; ++row) { + const float* d = dmat[row]; + + for (int col = 0; col < disparity_color_.cols; ++col) { + int index = (d[col] - min_disparity) * multiplier + 0.5; + index = std::min(255, std::max(0, index)); + // Fill as BGR + disparity_color_(row, col)[2] = colormap[3*index + 0]; + disparity_color_(row, col)[1] = colormap[3*index + 1]; + disparity_color_(row, col)[0] = colormap[3*index + 2]; + } + } + + // Must release the mutex before calling cv::imshow, or can deadlock against + // OpenCV's window mutex. + image_mutex_.unlock(); + if (!last_left_image_.empty()) { + cv::imshow("left", last_left_image_); + cv::waitKey(1); + } + if (!last_right_image_.empty()) { + cv::imshow("right", last_right_image_); + cv::waitKey(1); + } + cv::imshow("disparity", disparity_color_); + cv::waitKey(1); +} + +void StereoViewNode::saveImage(const char * prefix, const cv::Mat & image) +{ + if (!image.empty()) { + std::string filename = (filename_format_ % prefix % save_count_).str(); + cv::imwrite(filename, image); + RCLCPP_INFO(this->get_logger(), "Saved image %s", filename.c_str()); + } else { + RCLCPP_WARN(this->get_logger(), "Couldn't save %s image, no data!", prefix); + } +} + +void StereoViewNode::mouseCb(int event, int x, int y, int flags, void * param) +{ + (void)x; + (void)y; + (void)flags; + + if (event == cv::EVENT_LBUTTONDOWN) { + // RCLCPP_WARN_ONCE(this->get_logger(), "Left-clicking no longer saves images. Right-click instead."); + return; + } + + if (event != cv::EVENT_RBUTTONDOWN) { + return; + } + + StereoViewNode *sv = (StereoViewNode*)param; + std::lock_guard guard(sv->image_mutex_); + + sv->saveImage("left", sv->last_left_image_); + sv->saveImage("right", sv->last_right_image_); + sv->saveImage("disp", sv->disparity_color_); + sv->save_count_++; +} + +void StereoViewNode::checkInputsSynchronized() +{ + int threshold = 3 * all_received_; + if (left_received_ >= threshold || right_received_ >= threshold || disp_received_ >= threshold) { + RCLCPP_WARN( + this->get_logger(), + "[stereo_view] Low number of synchronized left/right/disparity triplets received.\n" + "Left images received: %d (topic '%s')\n" + "Right images received: %d (topic '%s')\n" + "Disparity images received: %d (topic '%s')\n" + "Synchronized triplets: %d\n" + "Possible issues:\n" + "\t* stereo_image_proc is not running.\n" + "\t Does `rosnode info %s` show any connections?\n" + "\t* The cameras are not synchronized.\n" + "\t Try restarting stereo_view with parameter _approximate_sync:=True\n" + "\t* The network is too slow. One or more images are dropped from each triplet.\n" + "\t Try restarting stereo_view, increasing parameter 'queue_size' (currently %d)", + left_received_, left_sub_.getTopic().c_str(), + right_received_, right_sub_.getTopic().c_str(), + disp_received_, disparity_sub_.getTopic().c_str(), + all_received_, this->get_name(), queue_size_); + } +} + +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_view::StereoViewNode) diff --git a/image_view/src/video_recorder.cpp b/image_view/src/video_recorder.cpp new file mode 100644 index 000000000..c3e9b361f --- /dev/null +++ b/image_view/src/video_recorder.cpp @@ -0,0 +1,33 @@ +// Copyright 2012, 2013, 2019 Open Source Robotics Foundation, 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 "image_view/video_recorder_node.hpp" + +int main(int argc, char** argv) +{ + using image_view::VideoRecorderNode; + + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto vr_node = std::make_shared(options); + + rclcpp::spin(vr_node); + + return 0; +} diff --git a/image_view/src/video_recorder_node.cpp b/image_view/src/video_recorder_node.cpp new file mode 100644 index 000000000..7fc7828ae --- /dev/null +++ b/image_view/src/video_recorder_node.cpp @@ -0,0 +1,145 @@ +// Copyright 2012, 2013, 2019 Open Source Robotics Foundation, 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 "image_view/video_recorder_node.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace image_view +{ + +VideoRecorderNode::VideoRecorderNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("video_recorder_node", options), + g_count(0), + g_last_wrote_time(rclcpp::Time(0)), + recording_started(false) +{ + bool stamped_filename; + + filename = this->declare_parameter("filename", std::string("output.avi")); + stamped_filename = this->declare_parameter("stamped_filename", false); + fps = this->declare_parameter("fps", 15); + codec = this->declare_parameter("codec", std::string("MJPG")); + encoding = this->declare_parameter("encoding", std::string("bgr8")); + // cv_bridge::CvtColorForDisplayOptions + min_depth_range = this->declare_parameter("min_depth_range", 0.0); + max_depth_range = this->declare_parameter("max_depth_range", 0.0); + use_dynamic_range = this->declare_parameter("use_dynamic_depth_range", false); + colormap = this->declare_parameter("colormap", -1); + + if (stamped_filename) { + std::size_t found = filename.find_last_of("/\\"); + std::string path = filename.substr(0, found + 1); + std::string basename = filename.substr(found + 1); + std::stringstream ss; + ss << this->now().nanoseconds() << basename; + filename = path + ss.str(); + RCLCPP_INFO(this->get_logger(), "Video recording to %s", filename.c_str()); + } + + if (codec.size() != 4) { + RCLCPP_ERROR(this->get_logger(), "The video codec must be a FOURCC identifier (4 chars)"); + rclcpp::shutdown(); + } + + auto topic = rclcpp::expand_topic_or_service_name("image", this->get_name(), this->get_namespace()); + sub_image = image_transport::create_subscription(this, topic, std::bind(&VideoRecorderNode::callback, this, std::placeholders::_1), "raw"); + + RCLCPP_INFO(this->get_logger(), "Waiting for topic %s...", topic.c_str()); +} + +VideoRecorderNode::~VideoRecorderNode() +{ + if (recording_started) { + std::cout << "\nVideo saved as: " << filename << std::endl; + } +} + +void VideoRecorderNode::callback(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg) +{ + if (!outputVideo.isOpened()) { + cv::Size size(image_msg->width, image_msg->height); + + outputVideo.open( + filename, + cv::VideoWriter::fourcc( + codec.c_str()[0], + codec.c_str()[1], + codec.c_str()[2], + codec.c_str()[3]), + fps, + size, + true); + + if (!outputVideo.isOpened()) { + RCLCPP_ERROR( + this->get_logger(), + "Could not create the output video! Check filename and/or support for codec."); + rclcpp::shutdown(); + } + + recording_started = true; + + RCLCPP_INFO( + this->get_logger(), + "Starting to record %s video at %ix%i@%i fps. Press Ctrl+C to stop recording.", + codec.c_str(), size.height, size.width, fps); + } + + if ((rclcpp::Time(image_msg->header.stamp) - g_last_wrote_time) < rclcpp::Duration(1.0 / fps)) { + // Skip to get video with correct fps + return; + } + + try { + cv_bridge::CvtColorForDisplayOptions options; + options.do_dynamic_scaling = use_dynamic_range; + options.min_image_value = min_depth_range; + options.max_image_value = max_depth_range; + options.colormap = colormap; + + const cv::Mat image = + cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg), encoding, options)->image; + + if (!image.empty()) { + outputVideo << image; + RCLCPP_INFO(this->get_logger(), "Recording frame %i\x1b[1F", g_count); + g_count++; + g_last_wrote_time = image_msg->header.stamp; + } else { + RCLCPP_WARN(this->get_logger(), "Frame skipped, no data!"); + } + } catch(cv_bridge::Exception) { + RCLCPP_ERROR( + this->get_logger(), + "Unable to convert %s image to %s", + image_msg->encoding.c_str(), encoding.c_str()); + return; + } +} + +} // namespace image_view + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_view::VideoRecorderNode)