From 0842fbe43107d6f2458abac4ce0769f6ed5b640e Mon Sep 17 00:00:00 2001 From: steve Date: Fri, 19 Apr 2019 11:20:42 +0800 Subject: [PATCH] [New Feature] add image view package. --- image_view/CHANGELOG.rst | 188 ++++++ image_view/CMakeLists.txt | 90 +++ image_view/cfg/ImageView.cfg | 29 + image_view/mainpage.dox | 11 + image_view/nodelet_plugins.xml | 11 + image_view/package.xml | 48 ++ image_view/rosdoc.yaml | 4 + image_view/scripts/extract_images_sync | 99 +++ image_view/src/nodelets/disparity_nodelet.cpp | 439 ++++++++++++++ image_view/src/nodelets/image_nodelet.cpp | 265 ++++++++ image_view/src/nodelets/window_thread.cpp | 52 ++ image_view/src/nodelets/window_thread.h | 44 ++ image_view/src/nodes/disparity_view.cpp | 54 ++ image_view/src/nodes/extract_images.cpp | 154 +++++ image_view/src/nodes/image_saver.cpp | 226 +++++++ image_view/src/nodes/image_view.cpp | 227 +++++++ image_view/src/nodes/stereo_view.cpp | 573 ++++++++++++++++++ image_view/src/nodes/video_recorder.cpp | 141 +++++ 18 files changed, 2655 insertions(+) create mode 100644 image_view/CHANGELOG.rst create mode 100644 image_view/CMakeLists.txt create mode 100644 image_view/cfg/ImageView.cfg create mode 100644 image_view/mainpage.dox create mode 100644 image_view/nodelet_plugins.xml create mode 100644 image_view/package.xml create mode 100644 image_view/rosdoc.yaml create mode 100644 image_view/scripts/extract_images_sync create mode 100644 image_view/src/nodelets/disparity_nodelet.cpp create mode 100644 image_view/src/nodelets/image_nodelet.cpp create mode 100644 image_view/src/nodelets/window_thread.cpp create mode 100644 image_view/src/nodelets/window_thread.h create mode 100644 image_view/src/nodes/disparity_view.cpp create mode 100644 image_view/src/nodes/extract_images.cpp create mode 100644 image_view/src/nodes/image_saver.cpp create mode 100644 image_view/src/nodes/image_view.cpp create mode 100644 image_view/src/nodes/stereo_view.cpp create mode 100644 image_view/src/nodes/video_recorder.cpp diff --git a/image_view/CHANGELOG.rst b/image_view/CHANGELOG.rst new file mode 100644 index 0000000..f5756fa --- /dev/null +++ b/image_view/CHANGELOG.rst @@ -0,0 +1,188 @@ +1.12.23 (2018-05-10) +-------------------- + +1.12.22 (2017-12-08) +-------------------- + +1.12.21 (2017-11-05) +-------------------- +* call namedWindow from same thread as imshow, need waitKay, now cvStartWindowThreads is null funciton on window_QT.h (`#279 `_) +* Contributors: Kei Okada + +1.12.20 (2017-04-30) +-------------------- +* DisparityViewNodelet: fixed freeze (`#244 `_) +* launch image view with a predefined window size (`#257 `_) +* Remove python-opencv run_depend for image_view (`#270 `_) + The `python-opencv` dependency pulls in the system OpenCV v2.4 which is + not required since the `image_view` package depends on `cv_bridge` which + pulls in `opencv3` and `opencv3` provides the python library that + `image_view` can use. +* Fix encoding error message (`#253 `_) + * Fix encoding error message + * Update image_saver.cpp + Allow compilation on older compilers +* Including stereo_msgs dep fixes `#248 `_ (`#249 `_) +* Add no gui mode to just visualize & publish with image_view (`#241 `_) +* stere_view: fixed empty left, right, disparity windows with opencv3 +* Apply value scaling to depth/float image with min/max image value + If min/max image value is specified we just use it, and if not, + - 32FC1: we assume depth image with meter metric, and 10[m] as the max range. + - 16UC1: we assume depth image with milimeter metric, and 10 * 1000[mm] as the max range. +* Depends on cv_bridge 1.11.13 for CvtColorForDisplayOptions + Close `#238 `_ +* fix doc jobs + This is a proper fix for `#233 `_ +* address gcc6 build error + With gcc6, compiling fails with `stdlib.h: No such file or directory`, + as including '-isystem /usr/include' breaks with gcc6, cf., + https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129. + This commit addresses this issue for this package in the same way + it was addressed in various other ROS packages. A list of related + commits and pull requests is at: + https://github.com/ros/rosdistro/issues/12783 + Signed-off-by: Lukas Bulwahn +* Contributors: Christopher Wecht, Kartik Mohta, Kei Okada, Kentaro Wada, Lukas Bulwahn, Leonard Gerard, Vincent Rabaud, cwecht, mryellow + +1.12.19 (2016-07-24) +-------------------- +* Add colormap option in video_recorder +* Merge pull request `#203 `_ from wkentaro/video-recorder-timestamp + [image_view] Stamped video output filename for video recorder +* bump version requirement for cv_bridge dep + Closes `#215 `_ +* Request for saving image with start/end two triggers +* Stamped video output filename + - _filename:=output.avi _stamped_filename:=false -> output.avi + - _filename:=_out.avi _stamped_filename:=true -> 1466299931.584632829_out.avi + - _filename:=$HOME/.ros/.avi _stamped_filename:=true -> /home/ubuntu/.ros/1466299931.584632829.avi +* Revert max_depth_range to default value for cvtColorForDisplay +* Contributors: Kentaro Wada, Vincent Rabaud + +1.12.18 (2016-07-12) +-------------------- +* Use image_transport::Subscriber aside from ros::Subscriber +* Refactor: Remove subscription of camera_info in video_recorder +* Add colormap options for displaying image topic +* Use CvtColorForDisplayOptions for cvtColorForDisplay +* Contributors: Kentaro Wada, Vincent Rabaud + +1.12.17 (2016-07-11) +-------------------- +* Fix timestamp to get correct fps in video_recorder +* Get correct fps in video_recorder.cpp +* Do dynamic scaling for float images +* Contributors: Kentaro Wada + +1.12.16 (2016-03-19) +-------------------- +* Remove code for roslib on .cfg files + Closes `#185 `_ +* add cv::waitKey for opencv3 installed from source to fix freezing issue +* when no image is saved, do not save camera info + When the images are not recorded because "save_all_image" is false and "save_image_service" is false, the frame count should not be incremented and the camera info should not be written to disk. +* Add std_srvs to catkin find_package() +* Contributors: Jeremy Kerfs, Jochen Sprickerhof, Kentaro Wada, Krishneel + +1.12.15 (2016-01-17) +-------------------- +* simplify the OpenCV dependency +* [image_view] Configure do_dynamic_scaling param with dynamic_reconfigure +* [image_view] Scale 16UC1 depth image +* fix compilation +* Extract images which are synchronized with message_filters +* [image_view] Show full path when failed to save image +* [image_view] Enable to specify transport with arg +* [image_view] feedback: no need threading for callback +* [image_view/image_view] Make as a node +* Added sensor_msgs::Image conversion to cv::Mat from rqt_image_view in + order to be able to create videos from kinect depth images (cv_bridge + currently doesn't support 16UC1 image encoding). + Code adapted from: + https://github.com/ros-visualization/rqt_common_plugins/blob/groovy-devel/rqt_image_view/src/rqt_image_view/image_view.cpp +* simplify OpenCV3 conversion +* use the color conversion for display from cv_bridge +* Contributors: Carlos Costa, Kentaro Wada, Vincent Rabaud + +1.12.14 (2015-07-22) +-------------------- +* reduce the differences between OpenCV2 and 3 +* do not build GUIs on Android + This fixes `#137 `_ +* Contributors: Vincent Rabaud + +1.12.13 (2015-04-06) +-------------------- + +1.12.12 (2014-12-31) +-------------------- +* Convert function to inline to avoid duplicates with image_transport +* Revert "remove GTK dependency" + This reverts commit a6e15e796a40385fbbf8da05966aa47d179dcb46. + Conflicts: + image_view/CMakeLists.txt + image_view/src/nodelets/disparity_nodelet.cpp + image_view/src/nodes/stereo_view.cpp +* Revert "make sure waitKey is called after imshow" + This reverts commit d13e3ed6af819459bca221ece779964a74beefac. +* Revert "brings back window_thread" + This reverts commit 41a655e8e99910c13a3e7f1ebfdd083207cef76f. +* Contributors: Gary Servin, Vincent Rabaud + +1.12.11 (2014-10-26) +-------------------- +* brings back window_thread + This fixes `#102 `_ fully +* small optimizations +* add the image_transport parameter +* Contributors: Vincent Rabaud + +1.12.10 (2014-09-28) +-------------------- + +1.12.9 (2014-09-21) +------------------- +* get code to compile with OpenCV3 + fixes `#96 `_ +* Contributors: Vincent Rabaud + +1.12.8 (2014-08-19) +------------------- + +1.12.6 (2014-07-27) +------------------- +* make sure waitKey is called after imshow +* remove GTK dependency +* small speedups +* Contributors: Vincent Rabaud + +1.12.5 (2014-05-11) +------------------- +* image_view: Add depend on gtk2 +* Contributors: Scott K Logan + +1.12.4 (2014-04-28) +------------------- +* fixes `#65 `_ +* Contributors: Vincent Rabaud + +1.12.3 (2014-04-12) +------------------- + +1.12.2 (2014-04-08) +------------------- + +1.12.1 (2014-04-06) +------------------- +* get proper opencv dependency +* Contributors: Vincent Rabaud + +1.11.7 (2014-03-28) +------------------- +* Added requirement for core. +* Contributors: Jonathan J Hunt + +1.11.3 (2013-10-06 20:21:55 +0100) +---------------------------------- +- #41: allow image_saver to save image topics +- #40: use proper download URL diff --git a/image_view/CMakeLists.txt b/image_view/CMakeLists.txt new file mode 100644 index 0000000..88113d1 --- /dev/null +++ b/image_view/CMakeLists.txt @@ -0,0 +1,90 @@ +cmake_minimum_required(VERSION 2.8) +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} +) + +# Extra tools +add_executable(extract_images src/nodes/extract_images.cpp) +target_link_libraries(extract_images ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +add_executable(image_saver src/nodes/image_saver.cpp) +target_link_libraries(image_saver ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +add_executable(video_recorder src/nodes/video_recorder.cpp) +target_link_libraries(video_recorder ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +install(TARGETS extract_images image_saver video_recorder + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Deal with the GUI's +if(ANDROID) + return() +endif() + +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} +) +install(TARGETS image_view + DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +# 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/nodes/disparity_view.cpp) +target_link_libraries(disparity_view ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +add_executable(stereo_view src/nodes/stereo_view.cpp) +target_link_libraries(stereo_view ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${GTK_LIBRARIES} + ${GTK2_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +install(TARGETS disparity_view image_view_exe stereo_view + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Python programs +catkin_install_python( + PROGRAMS scripts/extract_images_sync + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/image_view/cfg/ImageView.cfg b/image_view/cfg/ImageView.cfg new file mode 100644 index 0000000..8cc0888 --- /dev/null +++ b/image_view/cfg/ImageView.cfg @@ -0,0 +1,29 @@ +#! /usr/bin/env python + +PACKAGE='image_view' +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +edit_method_colormap = gen.enum([ + gen.const("NO_COLORMAP", int_t, -1, "NO_COLORMAP"), + gen.const("AUTUMN", int_t, 0, "COLORMAP_AUTUMN"), + gen.const("BONE", int_t, 1, "COLORMAP_BONE"), + gen.const("JET", int_t, 2, "COLORMAP_JET"), + gen.const("WINTER", int_t, 3, "COLORMAP_WINTER"), + gen.const("RAINBOW", int_t, 4, "COLORMAP_RAINBOW"), + gen.const("OCEAN", int_t, 5, "COLORMAP_OCEAN"), + gen.const("SUMMER", int_t, 6, "COLORMAP_SUMMER"), + gen.const("SPRING", int_t, 7, "COLORMAP_SPRING"), + gen.const("COOL", int_t, 8, "COLORMAP_COOL"), + gen.const("HSV", int_t, 9, "COLORMAP_HSV"), + gen.const("PINK", int_t, 10, "COLORMAP_PINK"), + gen.const("HOT", int_t, 11, "COLORMAP_HOT"), +], "colormap") + +gen.add('do_dynamic_scaling', bool_t, 0, 'Do dynamic scaling about pixel values or not', False) +gen.add('colormap', int_t, 0, "colormap", -1, -1, 11, edit_method=edit_method_colormap); +gen.add('min_image_value', double_t, 0, "Minimum image value for scaling depth/float image.", default=0, min=0); +gen.add('max_image_value', double_t, 0, "Maximum image value for scaling depth/float image.", default=0, min=0); + +exit(gen.generate(PACKAGE, 'image_view', 'ImageView')) diff --git a/image_view/mainpage.dox b/image_view/mainpage.dox new file mode 100644 index 0000000..87cfb37 --- /dev/null +++ b/image_view/mainpage.dox @@ -0,0 +1,11 @@ +/** +@mainpage image_view + +@htmlinclude manifest.html + +@b image_view is a simple utility for viewing an image topic. For usage see +http://www.ros.org/wiki/image_view. + +Currently this package has no public code API. + +*/ diff --git a/image_view/nodelet_plugins.xml b/image_view/nodelet_plugins.xml new file mode 100644 index 0000000..b6bd0fc --- /dev/null +++ b/image_view/nodelet_plugins.xml @@ -0,0 +1,11 @@ + + + + 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 new file mode 100644 index 0000000..1e3becb --- /dev/null +++ b/image_view/package.xml @@ -0,0 +1,48 @@ + + image_view + 1.12.23 + + A simple viewer for ROS image topics. Includes a specialized viewer + for stereo + disparity images. + + Patrick Mihelich + Vincent Rabaud + Steven Macenski + Autonomoustuff team + BSD + http://www.ros.org/wiki/image_view + + + + + + + catkin + + rostest + + camera_calibration_parsers + cv_bridge + dynamic_reconfigure + gtk2 + image_transport + message_filters + message_generation + nodelet + rosconsole + roscpp + sensor_msgs + std_srvs + stereo_msgs + + camera_calibration_parsers + cv_bridge + dynamic_reconfigure + gtk2 + image_transport + message_filters + nodelet + rosconsole + roscpp + std_srvs + diff --git a/image_view/rosdoc.yaml b/image_view/rosdoc.yaml new file mode 100644 index 0000000..976fdf0 --- /dev/null +++ b/image_view/rosdoc.yaml @@ -0,0 +1,4 @@ + - builder: doxygen + name: C++ API + output_dir: c++ + file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync new file mode 100644 index 0000000..cd86ddd --- /dev/null +++ b/image_view/scripts/extract_images_sync @@ -0,0 +1,99 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# Software License Agreement (BSD License) +# +# Copyright (c) 2015, 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. +"""Save images of multiple topics with timestamp synchronization. + +Usage: rosrun image_view extract_images_sync _inputs:='[, ]' +""" + +import sys + +import cv2 + +import cv_bridge +import message_filters +import rospy +from sensor_msgs.msg import Image + + +class ExtractImagesSync(object): + + def __init__(self): + self.seq = 0 + self.fname_fmt = rospy.get_param( + '~filename_format', 'frame%04i_%i.jpg') + self.do_dynamic_scaling = rospy.get_param( + '~do_dynamic_scaling', False) + img_topics = rospy.get_param('~inputs', None) + if img_topics is None: + rospy.logwarn("""\ +extract_images_sync: rosparam '~inputs' has not been specified! \ +Typical command-line usage: +\t$ rosrun image_view extract_images_sync _inputs:= +\t$ rosrun image_view extract_images_sync \ +_inputs:='[, ]'""") + sys.exit(1) + if not isinstance(img_topics, list): + img_topics = [img_topics] + subs = [] + for t in img_topics: + subs.append(message_filters.Subscriber(t, Image)) + if rospy.get_param('~approximate_sync', False): + sync = message_filters.ApproximateTimeSynchronizer( + subs, queue_size=100, slop=.1) + else: + sync = message_filters.TimeSynchronizer( + subs, queue_size=100) + sync.registerCallback(self.save) + + def save(self, *imgmsgs): + seq = self.seq + bridge = cv_bridge.CvBridge() + for i, imgmsg in enumerate(imgmsgs): + img = bridge.imgmsg_to_cv2(imgmsg) + channels = img.shape[2] if img.ndim == 3 else 1 + encoding_in = bridge.dtype_with_channels_to_cvtype2( + img.dtype, channels) + img = cv_bridge.cvtColorForDisplay( + img, encoding_in=encoding_in, encoding_out='', + do_dynamic_scaling=self.do_dynamic_scaling) + fname = self.fname_fmt % (seq, i) + print('Save image as {0}'.format(fname)) + cv2.imwrite(fname, img) + self.seq = seq + 1 + + +if __name__ == '__main__': + rospy.init_node('extract_images_sync') + extractor = ExtractImagesSync() + rospy.spin() diff --git a/image_view/src/nodelets/disparity_nodelet.cpp b/image_view/src/nodelets/disparity_nodelet.cpp new file mode 100644 index 0000000..e177a87 --- /dev/null +++ b/image_view/src/nodelets/disparity_nodelet.cpp @@ -0,0 +1,439 @@ +/********************************************************************* +* 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" + +#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 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 + + +namespace image_view { + +class DisparityNodelet : public nodelet::Nodelet +{ + // colormap for disparities, RGB order + static unsigned char colormap[]; + + std::string window_name_; + ros::Subscriber sub_; + cv::Mat_ disparity_color_; + bool initialized; + + virtual void onInit(); + + void imageCb(const stereo_msgs::DisparityImageConstPtr& msg); + +public: + ~DisparityNodelet(); +}; + +DisparityNodelet::~DisparityNodelet() +{ + cv::destroyWindow(window_name_); +} + +void DisparityNodelet::onInit() +{ + initialized = false; + ros::NodeHandle nh = getNodeHandle(); + ros::NodeHandle local_nh = getPrivateNodeHandle(); + const std::vector& argv = getMyArgv(); + + // 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(); + + // Default window name is the resolved topic name + std::string topic = nh.resolveName("image"); + local_nh.param("window_name", window_name_, topic); + + bool autosize; + local_nh.param("autosize", autosize, false); + + //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_ = nh.subscribe(topic, 1, &DisparityNodelet::imageCb, this); +} + +void DisparityNodelet::imageCb(const stereo_msgs::DisparityImageConstPtr& 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"); + 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()); + return; + } + + if(!initialized) { + 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); + 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; + 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)); + // Fill as BGR + (*disparity_color)[2] = colormap[3*index + 0]; + (*disparity_color)[1] = colormap[3*index + 1]; + (*disparity_color)[0] = colormap[3*index + 2]; + } + } + + /// @todo For Electric, consider option to draw outline of valid window +#if 0 + sensor_msgs::RegionOfInterest valid = msg->valid_window; + cv::Point tl(valid.x_offset, valid.y_offset), br(valid.x_offset + valid.width, valid.y_offset + valid.height); + cv::rectangle(disparity_color_, tl, br, CV_RGB(255,0,0), 1); +#endif + + cv::imshow(window_name_, disparity_color_); + cv::waitKey(10); +} + +unsigned char DisparityNodelet::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, + }; + +} // namespace image_view + +// Register the nodelet +#include +PLUGINLIB_EXPORT_CLASS( image_view::DisparityNodelet, nodelet::Nodelet) diff --git a/image_view/src/nodelets/image_nodelet.cpp b/image_view/src/nodelets/image_nodelet.cpp new file mode 100644 index 0000000..57467d7 --- /dev/null +++ b/image_view/src/nodelets/image_nodelet.cpp @@ -0,0 +1,265 @@ +/********************************************************************* +* 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/nodelets/window_thread.cpp b/image_view/src/nodelets/window_thread.cpp new file mode 100644 index 0000000..8e1c8eb --- /dev/null +++ b/image_view/src/nodelets/window_thread.cpp @@ -0,0 +1,52 @@ +/********************************************************************* +* 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 "window_thread.h" +#include +#include + +namespace { +void startWindowThreadLocal() { + cv::startWindowThread(); +} +} + +namespace image_view { + +void startWindowThread() +{ + static boost::once_flag cv_thread_flag = BOOST_ONCE_INIT; + boost::call_once(&startWindowThreadLocal, cv_thread_flag); +} + +} // namespace image_view diff --git a/image_view/src/nodelets/window_thread.h b/image_view/src/nodelets/window_thread.h new file mode 100644 index 0000000..9418e79 --- /dev/null +++ b/image_view/src/nodelets/window_thread.h @@ -0,0 +1,44 @@ +/********************************************************************* +* 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. +*********************************************************************/ +#ifndef IMAGE_VIEW_WINDOW_THREAD_H +#define IMAGE_VIEW_WINDOW_THREAD_H + +namespace image_view { + +// Makes absolutely sure we only start the OpenCV window thread once +void startWindowThread(); + +} // namespace image_view + +#endif diff --git a/image_view/src/nodes/disparity_view.cpp b/image_view/src/nodes/disparity_view.cpp new file mode 100644 index 0000000..d013b22 --- /dev/null +++ b/image_view/src/nodes/disparity_view.cpp @@ -0,0 +1,54 @@ +/********************************************************************* +* 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 + +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 + + manager.load(ros::this_node::getName(), "image_view/disparity", remappings, my_argv); + + ros::spin(); + return 0; +} diff --git a/image_view/src/nodes/extract_images.cpp b/image_view/src/nodes/extract_images.cpp new file mode 100644 index 0000000..9ebdd48 --- /dev/null +++ b/image_view/src/nodes/extract_images.cpp @@ -0,0 +1,154 @@ +/********************************************************************* +* 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 new file mode 100644 index 0000000..d3509c3 --- /dev/null +++ b/image_view/src/nodes/image_saver.cpp @@ -0,0 +1,226 @@ +/********************************************************************* +* 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 new file mode 100644 index 0000000..df7f993 --- /dev/null +++ b/image_view/src/nodes/image_view.cpp @@ -0,0 +1,227 @@ +/********************************************************************* +* 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 new file mode 100644 index 0000000..64a01cc --- /dev/null +++ b/image_view/src/nodes/stereo_view.cpp @@ -0,0 +1,573 @@ +/********************************************************************* +* 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 new file mode 100644 index 0000000..56b7a02 --- /dev/null +++ b/image_view/src/nodes/video_recorder.cpp @@ -0,0 +1,141 @@ +/**************************************************************************** +* 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; +}