diff --git a/ros/image_transport_plugin/.travis.yml b/ros/image_transport_plugin/.travis.yml new file mode 100644 index 0000000..463da47 --- /dev/null +++ b/ros/image_transport_plugin/.travis.yml @@ -0,0 +1,64 @@ +sudo: required +dist: trusty +language: generic +env: + - OPENCV_VERSION=2 ROS_DISTRO=indigo + - OPENCV_VERSION=3 ROS_DISTRO=indigo + - OPENCV_VERSION=2 ROS_DISTRO=jade + - OPENCV_VERSION=3 ROS_DISTRO=jade +# Install system dependencies, namely ROS. +before_install: + # Define some config vars. + - export ROS_CI_DESKTOP=`lsb_release -cs` # e.g. [precise|trusty] + - export CI_SOURCE_PATH=$(pwd) + - export REPOSITORY_NAME=${PWD##*/} + - export ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall + - export CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options + - export ROS_PARALLEL_JOBS='-j8 -l6' + # Install ROS + - sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" + - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + - sudo apt-get update -qq + # Install ROS + - sudo apt-get install -y python-catkin-pkg python-catkin-tools python-rosdep python-wstool ros-$ROS_DISTRO-catkin + - source /opt/ros/$ROS_DISTRO/setup.bash + # Setup for rosdep + - sudo rosdep init + - rosdep update + +# Create a catkin workspace with the package under test. +install: + - mkdir -p ~/catkin_ws/src + + # Add the package under test to the workspace. + - cd ~/catkin_ws/src + - ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace + + - if [ $OPENCV_VERSION == 3 ]; then sed -i 's@libopencv-dev@opencv3@' $REPOSITORY_NAME/*/package.xml ; fi + +# Install all dependencies, using wstool and rosdep. +# wstool looks for a ROSINSTALL_FILE defined in before_install. +before_script: + # source dependencies: install using wstool. + - cd ~/catkin_ws/src + - wstool init + - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi + - wstool up + + # package depdencies: install using rosdep. + - cd ~/catkin_ws + - rosdep install -q -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO + +# Compile and test. +script: + - source /opt/ros/$ROS_DISTRO/setup.bash + - cd ~/catkin_ws + - catkin build -p1 -j1 --no-status + - catkin run_tests -p1 -j1 + - catkin_test_results --all build + - catkin clean -b --yes + - catkin config --install + - catkin build -p1 -j1 --no-status +after_failure: + - find ${HOME}/.ros/test_results -type f -exec echo "== {} ==" \; -exec cat {} \; + - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/CHANGELOG.rst b/ros/image_transport_plugin/compressed_depth_image_transport/CHANGELOG.rst new file mode 100644 index 0000000..1581ecc --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/CHANGELOG.rst @@ -0,0 +1,138 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package compressed_depth_image_transport +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.9.5 (2016-10-03) +------------------ +* disable -Werr +* Contributors: Vincent Rabaud + +1.9.4 (2016-10-02) +------------------ +* address gcc6 build error and tune + 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 +* Fix a missing return statement and add -Wall -Werror. +* Contributors: Lukas Bulwahn, Mac Mason + +1.9.3 (2016-01-17) +------------------ +* Refactor the codec into its own .h and .cpp. +* remove useless tf dependencies +* Contributors: Mac Mason, Vincent Rabaud + +1.9.2 (2015-04-25) +------------------ +* use compression parameters for both depths + fixes `#12 `_ +* get code to compile with OpenCV3 +* Contributors: Vincent Rabaud + +1.9.1 (2014-07-18) +------------------ + +1.9.0 (2014-05-16) +------------------ + +1.8.21 (2013-06-27) +------------------- +* maintainer: david gossow +* Contributors: David Gossow + +1.8.20 (2013-03-18) +------------------- +* 1.8.19 -> 1.8.20 +* Contributors: Julius Kammerl + +1.8.19 (2013-02-24) +------------------- +* 1.8.18 -> 1.8.19 +* Contributors: Julius Kammerl + +1.8.18 (2013-02-07 17:59) +------------------------- +* 1.8.17 -> 1.8.18 +* fixing input format checks (enabling rgba, bgra) + minor fixes +* Contributors: Julius Kammerl + +1.8.17 (2013-01-18) +------------------- +* 1.8.16 -> 1.8.17 +* Contributors: Julius Kammerl + +1.8.16 (2013-01-17) +------------------- +* 1.8.15 -> 1.8.16 +* use the pluginlib script to remove some runtime warnings +* Contributors: Julius Kammerl, Vincent Rabaud + +1.8.15 (2012-12-28 20:11) +------------------------- + +1.8.14 (2012-12-28 20:02) +------------------------- + +1.8.13 (2012-12-28 19:06) +------------------------- +* fix the bad exports +* make sure the plugins are visible by image_transport +* added license headers to various cpp and h files +* Contributors: Aaron Blasdel, Vincent Rabaud + +1.8.12 (2012-12-19 19:30) +------------------------- +* fix downstream stuff in cmake +* Contributors: Dirk Thomas + +1.8.11 (2012-12-19 17:17) +------------------------- +* fix cmake order +* Contributors: Dirk Thomas + +1.8.10 (2012-12-19 17:03) +------------------------- +* fix dyn reconf +* Contributors: Dirk Thomas + +1.8.9 (2012-12-19 00:26) +------------------------ +* switching to verion 1.8.9 +* Contributors: Julius Kammerl + +1.8.8 (2012-12-17) +------------------ +* adding build_deb on message_generation & mrun_deb on message_runtime +* Updated package.xml for new buildtool_depend tag for catkin requirement +* Contributors: Julius Kammerl, mirzashah + +1.8.7 (2012-12-10 15:29) +------------------------ +* adding missing tf build dependency +* Contributors: Julius Kammerl + +1.8.6 (2012-12-10 15:08) +------------------------ +* switching to version 1.8.6 +* Contributors: Julius Kammerl + +1.8.5 (2012-12-09) +------------------ +* adding missing build debs +* added class_loader_hide_library_symbols macros to CMakeList +* switching to 1.8.5 +* Contributors: Julius Kammerl + +1.8.4 (2012-11-30) +------------------ +* switching to version 1.8.4 +* adding plugin.xml exports for pluginlib +* catkinizing theora_image_transport +* catkinizing compressed_depth_image_transport +* github migration from code.ros.org (r40053) +* Contributors: Julius Kammerl diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/CMakeLists.txt b/ros/image_transport_plugin/compressed_depth_image_transport/CMakeLists.txt new file mode 100644 index 0000000..6fe167a --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 2.8.3) +project(compressed_depth_image_transport) + +if (CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror") +endif() + +find_package(OpenCV REQUIRED) +find_package(catkin REQUIRED cv_bridge dynamic_reconfigure image_transport) + +# generate the dynamic_reconfigure config file +generate_dynamic_reconfigure_options(cfg/CompressedDepthPublisher.cfg) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS cv_bridge dynamic_reconfigure image_transport + DEPENDS OpenCV +) + +include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} src/compressed_depth_publisher.cpp src/compressed_depth_subscriber.cpp src/manifest.cpp src/codec.cpp) +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + +class_loader_hide_library_symbols(${PROJECT_NAME}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# add xml file +install(FILES compressed_depth_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/cfg/CompressedDepthPublisher.cfg b/ros/image_transport_plugin/compressed_depth_image_transport/cfg/CompressedDepthPublisher.cfg new file mode 100755 index 0000000..a5060e9 --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/cfg/CompressedDepthPublisher.cfg @@ -0,0 +1,14 @@ +#! /usr/bin/env python + +PACKAGE='compressed_depth_image_transport' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("depth_max", double_t, 0, "Maximum depth value (meter) ", 10 , 1, 100) +gen.add("depth_quantization", double_t, 0, "Depth value at which the sensor accuracy is 1 m (Kinect: >75)", 100, 1, 150) +gen.add("png_level", int_t, 0, "PNG compression level", 9, 1, 9) + + +exit(gen.generate(PACKAGE, "CompressedDepthPublisher", "CompressedDepthPublisher")) diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/compressed_depth_plugins.xml b/ros/image_transport_plugin/compressed_depth_image_transport/compressed_depth_plugins.xml new file mode 100644 index 0000000..f33cb13 --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/compressed_depth_plugins.xml @@ -0,0 +1,13 @@ + + + + This plugin publishes a compressed depth images using PNG compression. + + + + + + This plugin decodes a compressed depth images. + + + diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/codec.h b/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/codec.h new file mode 100644 index 0000000..213a925 --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/codec.h @@ -0,0 +1,57 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2012, Willow Garage. +* Copyright (c) 2016, Google, 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 "sensor_msgs/CompressedImage.h" +#include "sensor_msgs/Image.h" +#include "sensor_msgs/image_encodings.h" + +// Encoding and decoding of compressed depth images. +namespace compressed_depth_image_transport +{ + +// Returns a null pointer on bad input. +sensor_msgs::Image::Ptr decodeCompressedDepthImage(const sensor_msgs::CompressedImage& compressed_image); + +// Compress a depth image. The png_compression parameter is passed straight through to +// OpenCV as IMWRITE_PNG_COMPRESSION. Returns a null pointer on bad input. +sensor_msgs::CompressedImage::Ptr encodeCompressedDepthImage( + const sensor_msgs::Image& message, + double depth_max, + double depth_quantization, + int png_level); + +} // namespace compressed_depth_image_transport diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_publisher.h b/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_publisher.h new file mode 100644 index 0000000..9498261 --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_publisher.h @@ -0,0 +1,70 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "image_transport/simple_publisher_plugin.h" +#include +#include +#include + +namespace compressed_depth_image_transport { + +class CompressedDepthPublisher : public image_transport::SimplePublisherPlugin +{ +public: + virtual ~CompressedDepthPublisher() {} + + virtual std::string getTransportName() const + { + return "compressedDepth"; + } + +protected: + // Overridden to set up reconfigure server + virtual void advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size, + const image_transport::SubscriberStatusCallback &user_connect_cb, + const image_transport::SubscriberStatusCallback &user_disconnect_cb, + const ros::VoidPtr &tracked_object, bool latch); + + virtual void publish(const sensor_msgs::Image& message, + const PublishFn& publish_fn) const; + + typedef compressed_depth_image_transport::CompressedDepthPublisherConfig Config; + typedef dynamic_reconfigure::Server ReconfigureServer; + boost::shared_ptr reconfigure_server_; + Config config_; + + void configCb(Config& config, uint32_t level); +}; + +} //namespace compressed_depth_image_transport diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_subscriber.h b/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_subscriber.h new file mode 100644 index 0000000..4ccf5a3 --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_subscriber.h @@ -0,0 +1,55 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "image_transport/simple_subscriber_plugin.h" +#include + +namespace compressed_depth_image_transport { + +class CompressedDepthSubscriber : public image_transport::SimpleSubscriberPlugin +{ +public: + virtual ~CompressedDepthSubscriber() {} + + virtual std::string getTransportName() const + { + return "compressedDepth"; + } + +protected: + virtual void internalCallback(const sensor_msgs::CompressedImageConstPtr& message, + const Callback& user_cb); +}; + +} //namespace compressed_depth_image_transport diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/compression_common.h b/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/compression_common.h new file mode 100644 index 0000000..d3e84df --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/include/compressed_depth_image_transport/compression_common.h @@ -0,0 +1,58 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 COMPRESSED_DEPTH_IMAGE_TRANSPORT_COMPRESSION_COMMON +#define COMPRESSED_DEPTH_IMAGE_TRANSPORT_COMPRESSION_COMMON + +namespace compressed_depth_image_transport +{ + +// Compression formats +enum compressionFormat +{ + UNDEFINED = -1, INV_DEPTH +}; + +// Compression configuration +struct ConfigHeader +{ + // compression format + compressionFormat format; + // quantization parameters (used in depth image compression) + float depthParam[2]; +}; + +} //namespace compressed_depth_image_transport + +#endif diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/package.xml b/ros/image_transport_plugin/compressed_depth_image_transport/package.xml new file mode 100644 index 0000000..c224974 --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/package.xml @@ -0,0 +1,28 @@ + + compressed_depth_image_transport + 1.9.5 + + Compressed_depth_image_transport provides a plugin to image_transport for transparently sending + depth images (raw, floating-point) using PNG compression. + + David Gossow + BSD + + http://www.ros.org/wiki/image_transport_plugins + Julius Kammerl + + catkin + + cv_bridge + dynamic_reconfigure + image_transport + + cv_bridge + dynamic_reconfigure + image_transport + + + + + + diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/src/codec.cpp b/ros/image_transport_plugin/compressed_depth_image_transport/src/codec.cpp new file mode 100644 index 0000000..27d2038 --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/src/codec.cpp @@ -0,0 +1,336 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2012 Willow Garage. +* Copyright (c) 2016 Google, 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 "cv_bridge/cv_bridge.h" +#include "compressed_depth_image_transport/codec.h" +#include "compressed_depth_image_transport/compression_common.h" +#include "ros/ros.h" + +// If OpenCV3 +#ifndef CV_VERSION_EPOCH +#include +#endif + +namespace enc = sensor_msgs::image_encodings; +using namespace cv; + +// Encoding and decoding of compressed depth images. +namespace compressed_depth_image_transport +{ + +sensor_msgs::Image::Ptr decodeCompressedDepthImage(const sensor_msgs::CompressedImage& message) +{ + + cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage); + + // Copy message header + cv_ptr->header = message.header; + + // Assign image encoding + std::string image_encoding = message.format.substr(0, message.format.find(';')); + cv_ptr->encoding = image_encoding; + + // Decode message data + if (message.data.size() > sizeof(ConfigHeader)) + { + + // Read compression type from stream + ConfigHeader compressionConfig; + memcpy(&compressionConfig, &message.data[0], sizeof(compressionConfig)); + + // Get compressed image data + const std::vector imageData(message.data.begin() + sizeof(compressionConfig), message.data.end()); + + // Depth map decoding + float depthQuantA, depthQuantB; + + // Read quantization parameters + depthQuantA = compressionConfig.depthParam[0]; + depthQuantB = compressionConfig.depthParam[1]; + + if (enc::bitDepth(image_encoding) == 32) + { + cv::Mat decompressed; + try + { + // Decode image data + decompressed = cv::imdecode(imageData, cv::IMREAD_UNCHANGED); + } + catch (cv::Exception& e) + { + ROS_ERROR("%s", e.what()); + return sensor_msgs::Image::Ptr(); + } + + size_t rows = decompressed.rows; + size_t cols = decompressed.cols; + + if ((rows > 0) && (cols > 0)) + { + cv_ptr->image = Mat(rows, cols, CV_32FC1); + + // Depth conversion + MatIterator_ itDepthImg = cv_ptr->image.begin(), + itDepthImg_end = cv_ptr->image.end(); + MatConstIterator_ itInvDepthImg = decompressed.begin(), + itInvDepthImg_end = decompressed.end(); + + for (; (itDepthImg != itDepthImg_end) && (itInvDepthImg != itInvDepthImg_end); ++itDepthImg, ++itInvDepthImg) + { + // check for NaN & max depth + if (*itInvDepthImg) + { + *itDepthImg = depthQuantA / ((float)*itInvDepthImg - depthQuantB); + } + else + { + *itDepthImg = std::numeric_limits::quiet_NaN(); + } + } + + // Publish message to user callback + return cv_ptr->toImageMsg(); + } + } + else + { + // Decode raw image + try + { + cv_ptr->image = cv::imdecode(imageData, CV_LOAD_IMAGE_UNCHANGED); + } + catch (cv::Exception& e) + { + ROS_ERROR("%s", e.what()); + return sensor_msgs::Image::Ptr(); + } + + size_t rows = cv_ptr->image.rows; + size_t cols = cv_ptr->image.cols; + + if ((rows > 0) && (cols > 0)) + { + // Publish message to user callback + return cv_ptr->toImageMsg(); + } + } + } + return sensor_msgs::Image::Ptr(); +} + +sensor_msgs::CompressedImage::Ptr encodeCompressedDepthImage( + const sensor_msgs::Image& message, + double depth_max, double depth_quantization, int png_level) +{ + + // Compressed image message + sensor_msgs::CompressedImage::Ptr compressed(new sensor_msgs::CompressedImage()); + compressed->header = message.header; + compressed->format = message.encoding; + + // Compression settings + std::vector params; + params.resize(3, 0); + + // Bit depth of image encoding + int bitDepth = enc::bitDepth(message.encoding); + int numChannels = enc::numChannels(message.encoding); + + // Image compression configuration + ConfigHeader compressionConfig; + compressionConfig.format = INV_DEPTH; + + // Compressed image data + std::vector compressedImage; + + // Update ros message format header + compressed->format += "; compressedDepth"; + + // Check input format + params[0] = cv::IMWRITE_PNG_COMPRESSION; + params[1] = png_level; + + if ((bitDepth == 32) && (numChannels == 1)) + { + float depthZ0 = depth_quantization; + float depthMax = depth_max; + + // OpenCV-ROS bridge + cv_bridge::CvImagePtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvCopy(message); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("%s", e.what()); + return sensor_msgs::CompressedImage::Ptr(); + } + + const Mat& depthImg = cv_ptr->image; + size_t rows = depthImg.rows; + size_t cols = depthImg.cols; + + if ((rows > 0) && (cols > 0)) + { + // Allocate matrix for inverse depth (disparity) coding + Mat invDepthImg(rows, cols, CV_16UC1); + + // Inverse depth quantization parameters + float depthQuantA = depthZ0 * (depthZ0 + 1.0f); + float depthQuantB = 1.0f - depthQuantA / depthMax; + + // Matrix iterators + MatConstIterator_ itDepthImg = depthImg.begin(), + itDepthImg_end = depthImg.end(); + MatIterator_ itInvDepthImg = invDepthImg.begin(), + itInvDepthImg_end = invDepthImg.end(); + + // Quantization + for (; (itDepthImg != itDepthImg_end) && (itInvDepthImg != itInvDepthImg_end); ++itDepthImg, ++itInvDepthImg) + { + // check for NaN & max depth + if (*itDepthImg < depthMax) + { + *itInvDepthImg = depthQuantA / *itDepthImg + depthQuantB; + } + else + { + *itInvDepthImg = 0; + } + } + + // Add coding parameters to header + compressionConfig.depthParam[0] = depthQuantA; + compressionConfig.depthParam[1] = depthQuantB; + + try + { + // Compress quantized disparity image + if (cv::imencode(".png", invDepthImg, compressedImage, params)) + { + float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) + / (float)compressedImage.size(); + ROS_DEBUG("Compressed Depth Image Transport - Compression: 1:%.2f (%lu bytes)", cRatio, compressedImage.size()); + } + else + { + ROS_ERROR("cv::imencode (png) failed on input image"); + return sensor_msgs::CompressedImage::Ptr(); + } + } + catch (cv::Exception& e) + { + ROS_ERROR("%s", e.msg.c_str()); + return sensor_msgs::CompressedImage::Ptr(); + } + } + } + // Raw depth map compression + else if ((bitDepth == 16) && (numChannels == 1)) + { + // OpenCV-ROS bridge + cv_bridge::CvImagePtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvCopy(message); + } + catch (Exception& e) + { + ROS_ERROR("%s", e.msg.c_str()); + return sensor_msgs::CompressedImage::Ptr(); + } + + const Mat& depthImg = cv_ptr->image; + size_t rows = depthImg.rows; + size_t cols = depthImg.cols; + + if ((rows > 0) && (cols > 0)) + { + unsigned short depthMaxUShort = static_cast(depth_max * 1000.0f); + + // Matrix iterators + MatIterator_ itDepthImg = cv_ptr->image.begin(), + itDepthImg_end = cv_ptr->image.end(); + + // Max depth filter + for (; itDepthImg != itDepthImg_end; ++itDepthImg) + { + if (*itDepthImg > depthMaxUShort) + *itDepthImg = 0; + } + + // Compress raw depth image + if (cv::imencode(".png", cv_ptr->image, compressedImage, params)) + { + float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) + / (float)compressedImage.size(); + ROS_DEBUG("Compressed Depth Image Transport - Compression: 1:%.2f (%lu bytes)", cRatio, compressedImage.size()); + } + else + { + ROS_ERROR("cv::imencode (png) failed on input image"); + return sensor_msgs::CompressedImage::Ptr(); + } + } + } + else + { + ROS_ERROR("Compressed Depth Image Transport - Compression requires single-channel 32bit-floating point or 16bit raw depth images (input format is: %s).", message.encoding.c_str()); + return sensor_msgs::CompressedImage::Ptr(); + } + + if (compressedImage.size() > 0) + { + // Add configuration to binary output + compressed->data.resize(sizeof(ConfigHeader)); + memcpy(&compressed->data[0], &compressionConfig, sizeof(ConfigHeader)); + + // Add compressed binary data to messages + compressed->data.insert(compressed->data.end(), compressedImage.begin(), compressedImage.end()); + + return compressed; + } + + return sensor_msgs::CompressedImage::Ptr(); +} + +} // namespace compressed_depth_image_transport diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/src/compressed_depth_publisher.cpp b/ros/image_transport_plugin/compressed_depth_image_transport/src/compressed_depth_publisher.cpp new file mode 100644 index 0000000..60c214b --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/src/compressed_depth_publisher.cpp @@ -0,0 +1,85 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "compressed_depth_image_transport/compressed_depth_publisher.h" +#include +#include +#include +#include + +#include "compressed_depth_image_transport/codec.h" +#include "compressed_depth_image_transport/compression_common.h" + +#include +#include + +using namespace cv; +using namespace std; + +namespace enc = sensor_msgs::image_encodings; + +namespace compressed_depth_image_transport +{ + +void CompressedDepthPublisher::advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size, + const image_transport::SubscriberStatusCallback &user_connect_cb, + const image_transport::SubscriberStatusCallback &user_disconnect_cb, + const ros::VoidPtr &tracked_object, bool latch) +{ + typedef image_transport::SimplePublisherPlugin Base; + Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch); + + // Set up reconfigure server for this topic + reconfigure_server_ = boost::make_shared(this->nh()); + ReconfigureServer::CallbackType f = boost::bind(&CompressedDepthPublisher::configCb, this, _1, _2); + reconfigure_server_->setCallback(f); +} + +void CompressedDepthPublisher::configCb(Config& config, uint32_t level) +{ + config_ = config; +} + +void CompressedDepthPublisher::publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const +{ + sensor_msgs::CompressedImage::Ptr compressed_image = + encodeCompressedDepthImage(message, config_.depth_max, config_.depth_quantization, config_.png_level); + + if (compressed_image) + { + publish_fn(*compressed_image); + } +} + +} //namespace compressed_depth_image_transport diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/src/compressed_depth_subscriber.cpp b/ros/image_transport_plugin/compressed_depth_image_transport/src/compressed_depth_subscriber.cpp new file mode 100644 index 0000000..33ef2a8 --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/src/compressed_depth_subscriber.cpp @@ -0,0 +1,53 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "compressed_depth_image_transport/compressed_depth_subscriber.h" + +#include "compressed_depth_image_transport/codec.h" +#include "compressed_depth_image_transport/compression_common.h" + +namespace compressed_depth_image_transport +{ + +void CompressedDepthSubscriber::internalCallback(const sensor_msgs::CompressedImageConstPtr& message, + const Callback& user_cb) +{ + sensor_msgs::Image::Ptr image = decodeCompressedDepthImage(*message); + if (image) + { + user_cb(image); + } +} + +} //namespace compressed_depth_image_transport diff --git a/ros/image_transport_plugin/compressed_depth_image_transport/src/manifest.cpp b/ros/image_transport_plugin/compressed_depth_image_transport/src/manifest.cpp new file mode 100644 index 0000000..51006c3 --- /dev/null +++ b/ros/image_transport_plugin/compressed_depth_image_transport/src/manifest.cpp @@ -0,0 +1,41 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "compressed_depth_image_transport/compressed_depth_publisher.h" +#include "compressed_depth_image_transport/compressed_depth_subscriber.h" + +PLUGINLIB_EXPORT_CLASS( compressed_depth_image_transport::CompressedDepthPublisher, image_transport::PublisherPlugin) + +PLUGINLIB_EXPORT_CLASS( compressed_depth_image_transport::CompressedDepthSubscriber, image_transport::SubscriberPlugin) diff --git a/ros/image_transport_plugin/compressed_image_transport/CHANGELOG.rst b/ros/image_transport_plugin/compressed_image_transport/CHANGELOG.rst new file mode 100644 index 0000000..17f71c6 --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/CHANGELOG.rst @@ -0,0 +1,154 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package compressed_image_transport +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.9.5 (2016-10-03) +------------------ + +1.9.4 (2016-10-02) +------------------ +* address gcc6 build error and tune + 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: Lukas Bulwahn + +1.9.3 (2016-01-17) +------------------ +* remove useless tf dependencies +* Using cfg-defined constants +* Changed flag name, and corrected typo in flag use. +* using IMREAD flags. +* Updated for indigo-devel +* Contributors: Cedric Pradalier, Vincent Rabaud + +1.9.2 (2015-04-25) +------------------ +* get code to compile with OpenCV3 +* avoid yet another image copy +* avoid copying data if it can be shared +* Contributors: Vincent Rabaud + +1.9.1 (2014-07-18) +------------------ + +1.9.0 (2014-05-16) +------------------ + +1.8.21 (2013-06-27) +------------------- +* maintainer: david gossow +* Contributors: David Gossow + +1.8.20 (2013-03-18) +------------------- +* 1.8.19 -> 1.8.20 +* Contributors: Julius Kammerl + +1.8.19 (2013-02-24) +------------------- +* 1.8.18 -> 1.8.19 +* Contributors: Julius Kammerl + +1.8.18 (2013-02-07 17:59) +------------------------- +* 1.8.17 -> 1.8.18 +* fixing input format checks (enabling rgba, bgra) + minor fixes +* Contributors: Julius Kammerl + +1.8.17 (2013-01-18) +------------------- +* 1.8.16 -> 1.8.17 +* Contributors: Julius Kammerl + +1.8.16 (2013-01-17) +------------------- +* 1.8.15 -> 1.8.16 +* use the pluginlib script to remove some runtime warnings +* Contributors: Julius Kammerl, Vincent Rabaud + +1.8.15 (2012-12-28 20:11) +------------------------- +* fix typo +* Contributors: Vincent Rabaud + +1.8.14 (2012-12-28 20:02) +------------------------- +* fix the bad xml naming +* Contributors: Vincent Rabaud + +1.8.13 (2012-12-28 19:06) +------------------------- +* fix the bad exports +* make sure the plugins are visible by image_transport +* added license headers to various cpp and h files +* Contributors: Aaron Blasdel, Vincent Rabaud + +1.8.12 (2012-12-19 19:30) +------------------------- +* fix downstream stuff in cmake +* Contributors: Dirk Thomas + +1.8.11 (2012-12-19 17:17) +------------------------- +* fix cmake order +* Contributors: Dirk Thomas + +1.8.10 (2012-12-19 17:03) +------------------------- +* fix dyn reconf +* Contributors: Dirk Thomas + +1.8.9 (2012-12-19 00:26) +------------------------ +* switching to verion 1.8.9 +* fixing dynamic_reconfigure related catkin errors +* Contributors: Julius Kammerl + +1.8.8 (2012-12-17) +------------------ +* adding build_deb on message_generation & mrun_deb on message_runtime +* Updated package.xml for new buildtool_depend tag for catkin requirement +* Contributors: Julius Kammerl, mirzashah + +1.8.7 (2012-12-10 15:29) +------------------------ +* adding missing tf build dependency +* Contributors: Julius Kammerl + +1.8.6 (2012-12-10 15:08) +------------------------ +* switching to version 1.8.6 +* Contributors: Julius Kammerl + +1.8.5 (2012-12-09) +------------------ +* adding missing build debs +* added class_loader_hide_library_symbols macros to CMakeList +* switching to 1.8.5 +* fixing compressed color format to comply with opencv api +* Contributors: Julius Kammerl + +1.8.4 (2012-11-30) +------------------ +* switching to version 1.8.4 +* adding plugin.xml exports for pluginlib +* catkinizing theora_image_transport +* github migration from code.ros.org (r40053) +* image_transport_plugins: Updated manifests to have better summaries, correct URLs. +* compressed_image_transport: Some todos. +* compressed_image_transport: Copy connection header to output Image, `#4250 `_. +* Added Ubuntu platform tags to manifest +* compressed_image_transport: Fixed swapping of R & B channels in data field. +* compressed_image_transport: Fixed bug in lookup of format parameter. +* getParam -> getParamCached +* Switch to opencv2 +* compressed_image_transport: Renamed parameters, which are now searched up the parameter tree. +* compressed_image_transport: Updated for compatibility with post-0.1 image_transport. +* image_transport_plugins: Initial stack check-in. Includes theora_image_transport, compressed_image_transport and libtheora. Currently depends on opencv, but may excise this in the future. +* Contributors: Julius Kammerl, gerkey, jamesb, mihelich, pmihelich diff --git a/ros/image_transport_plugin/compressed_image_transport/CMakeLists.txt b/ros/image_transport_plugin/compressed_image_transport/CMakeLists.txt new file mode 100644 index 0000000..a2e26ae --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 2.8.3) +project(compressed_image_transport) + +find_package(OpenCV REQUIRED) +find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure image_transport) + +# generate the dynamic_reconfigure config file +generate_dynamic_reconfigure_options(cfg/CompressedPublisher.cfg cfg/CompressedSubscriber.cfg) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS cv_bridge dynamic_reconfigure image_transport + DEPENDS OpenCV +) + +include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} src/compressed_publisher.cpp src/compressed_subscriber.cpp src/manifest.cpp) +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + +class_loader_hide_library_symbols(${PROJECT_NAME}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# add xml file +install(FILES compressed_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/ros/image_transport_plugin/compressed_image_transport/cfg/CompressedPublisher.cfg b/ros/image_transport_plugin/compressed_image_transport/cfg/CompressedPublisher.cfg new file mode 100755 index 0000000..856ba59 --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/cfg/CompressedPublisher.cfg @@ -0,0 +1,16 @@ +#! /usr/bin/env python + +PACKAGE='compressed_image_transport' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +format_enum = gen.enum( [gen.const("jpeg", str_t, "jpeg", "JPEG lossy compression"), + gen.const("png", str_t, "png", "PNG lossless compression")], + "Enum to set the compression format" ) +gen.add("format", str_t, 0, "Compression format", "jpeg", edit_method = format_enum) +gen.add("jpeg_quality", int_t, 0, "JPEG quality percentile", 80, 1, 100) +gen.add("png_level", int_t, 0, "PNG compression level", 9, 1, 9) + +exit(gen.generate(PACKAGE, "CompressedPublisher", "CompressedPublisher")) diff --git a/ros/image_transport_plugin/compressed_image_transport/cfg/CompressedSubscriber.cfg b/ros/image_transport_plugin/compressed_image_transport/cfg/CompressedSubscriber.cfg new file mode 100755 index 0000000..454f630 --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/cfg/CompressedSubscriber.cfg @@ -0,0 +1,15 @@ +#! /usr/bin/env python + +PACKAGE='compressed_image_transport' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +mode_enum = gen.enum( [gen.const("unchanged", str_t, "unchanged", "keep image encoding"), + gen.const("gray", str_t, "gray", "decode to gray"), + gen.const("color", str_t, "color", "decode to color")], + "Enum to set the decompression color mode" ) +gen.add("mode", str_t, 0, "Color Mode", "unchanged", edit_method = mode_enum) + +exit(gen.generate(PACKAGE, "CompressedSubscriber", "CompressedSubscriber")) diff --git a/ros/image_transport_plugin/compressed_image_transport/compressed_plugins.xml b/ros/image_transport_plugin/compressed_image_transport/compressed_plugins.xml new file mode 100644 index 0000000..d748d63 --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/compressed_plugins.xml @@ -0,0 +1,13 @@ + + + + This plugin publishes a CompressedImage using either JPEG or PNG compression. + + + + + + This plugin decompresses a CompressedImage topic. + + + diff --git a/ros/image_transport_plugin/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h b/ros/image_transport_plugin/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h new file mode 100644 index 0000000..39a4180 --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h @@ -0,0 +1,70 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "image_transport/simple_publisher_plugin.h" +#include +#include +#include + +namespace compressed_image_transport { + +class CompressedPublisher : public image_transport::SimplePublisherPlugin +{ +public: + virtual ~CompressedPublisher() {} + + virtual std::string getTransportName() const + { + return "compressed"; + } + +protected: + // Overridden to set up reconfigure server + virtual void advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size, + const image_transport::SubscriberStatusCallback &user_connect_cb, + const image_transport::SubscriberStatusCallback &user_disconnect_cb, + const ros::VoidPtr &tracked_object, bool latch); + + virtual void publish(const sensor_msgs::Image& message, + const PublishFn& publish_fn) const; + + typedef compressed_image_transport::CompressedPublisherConfig Config; + typedef dynamic_reconfigure::Server ReconfigureServer; + boost::shared_ptr reconfigure_server_; + Config config_; + + void configCb(Config& config, uint32_t level); +}; + +} //namespace compressed_image_transport diff --git a/ros/image_transport_plugin/compressed_image_transport/include/compressed_image_transport/compressed_subscriber.h b/ros/image_transport_plugin/compressed_image_transport/include/compressed_image_transport/compressed_subscriber.h new file mode 100644 index 0000000..4d5bf2f --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/include/compressed_image_transport/compressed_subscriber.h @@ -0,0 +1,71 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "image_transport/simple_subscriber_plugin.h" +#include +#include +#include + +namespace compressed_image_transport { + +class CompressedSubscriber : public image_transport::SimpleSubscriberPlugin +{ +public: + virtual ~CompressedSubscriber() {} + + virtual std::string getTransportName() const + { + return "compressed"; + } + +protected: + // Overridden to set up reconfigure server + virtual void subscribeImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const Callback& callback, const ros::VoidPtr& tracked_object, + const image_transport::TransportHints& transport_hints); + + + virtual void internalCallback(const sensor_msgs::CompressedImageConstPtr& message, + const Callback& user_cb); + + typedef compressed_image_transport::CompressedSubscriberConfig Config; + typedef dynamic_reconfigure::Server ReconfigureServer; + boost::shared_ptr reconfigure_server_; + Config config_; + int imdecode_flag_; + + void configCb(Config& config, uint32_t level); +}; + +} //namespace image_transport diff --git a/ros/image_transport_plugin/compressed_image_transport/include/compressed_image_transport/compression_common.h b/ros/image_transport_plugin/compressed_image_transport/include/compressed_image_transport/compression_common.h new file mode 100644 index 0000000..a680a09 --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/include/compressed_image_transport/compression_common.h @@ -0,0 +1,49 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 COMPRESSED_IMAGE_TRANSPORT_COMPRESSION_COMMON +#define COMPRESSED_IMAGE_TRANSPORT_COMPRESSION_COMMON + +namespace compressed_image_transport +{ + +// Compression formats +enum compressionFormat +{ + UNDEFINED = -1, JPEG, PNG +}; + +} //namespace compressed_image_transport + +#endif diff --git a/ros/image_transport_plugin/compressed_image_transport/package.xml b/ros/image_transport_plugin/compressed_image_transport/package.xml new file mode 100644 index 0000000..3882986 --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/package.xml @@ -0,0 +1,29 @@ + + compressed_image_transport + 1.9.5 + + Compressed_image_transport provides a plugin to image_transport for transparently sending images + encoded as JPEG or PNG. + + David Gossow + BSD + + http://www.ros.org/wiki/image_transport_plugins + Patrick Mihelich + Julius Kammerl + + catkin + + cv_bridge + dynamic_reconfigure + image_transport + + cv_bridge + dynamic_reconfigure + image_transport + + + + + + diff --git a/ros/image_transport_plugin/compressed_image_transport/src/compressed_publisher.cpp b/ros/image_transport_plugin/compressed_image_transport/src/compressed_publisher.cpp new file mode 100644 index 0000000..026d6e9 --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/src/compressed_publisher.cpp @@ -0,0 +1,221 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "compressed_image_transport/compressed_publisher.h" +#include +#include +#include +#include + +#include "compressed_image_transport/compression_common.h" + +#include +#include + +using namespace cv; +using namespace std; + +namespace enc = sensor_msgs::image_encodings; + +namespace compressed_image_transport +{ + +void CompressedPublisher::advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size, + const image_transport::SubscriberStatusCallback &user_connect_cb, + const image_transport::SubscriberStatusCallback &user_disconnect_cb, + const ros::VoidPtr &tracked_object, bool latch) +{ + typedef image_transport::SimplePublisherPlugin Base; + Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch); + + // Set up reconfigure server for this topic + reconfigure_server_ = boost::make_shared(this->nh()); + ReconfigureServer::CallbackType f = boost::bind(&CompressedPublisher::configCb, this, _1, _2); + reconfigure_server_->setCallback(f); +} + +void CompressedPublisher::configCb(Config& config, uint32_t level) +{ + config_ = config; +} + +void CompressedPublisher::publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const +{ + // Compressed image message + sensor_msgs::CompressedImage compressed; + compressed.header = message.header; + compressed.format = message.encoding; + + // Compression settings + std::vector params; + params.resize(3, 0); + + // Get codec configuration + compressionFormat encodingFormat = UNDEFINED; + if (config_.format == compressed_image_transport::CompressedPublisher_jpeg) + encodingFormat = JPEG; + if (config_.format == compressed_image_transport::CompressedPublisher_png) + encodingFormat = PNG; + + // Bit depth of image encoding + int bitDepth = enc::bitDepth(message.encoding); + int numChannels = enc::numChannels(message.encoding); + + switch (encodingFormat) + { + // JPEG Compression + case JPEG: + { + params[0] = CV_IMWRITE_JPEG_QUALITY; + params[1] = config_.jpeg_quality; + + // Update ros message format header + compressed.format += "; jpeg compressed "; + + // Check input format + if ((bitDepth == 8) || (bitDepth == 16)) + { + // Target image format + std::string targetFormat; + if (enc::isColor(message.encoding)) + { + // convert color images to BGR8 format + targetFormat = "bgr8"; + compressed.format += targetFormat; + } + + // OpenCV-ros bridge + try + { + boost::shared_ptr tracked_object; + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat); + + // Compress image + if (cv::imencode(".jpg", cv_ptr->image, compressed.data, params)) + { + + float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) + / (float)compressed.data.size(); + ROS_DEBUG("Compressed Image Transport - Codec: jpg, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed.data.size()); + } + else + { + ROS_ERROR("cv::imencode (jpeg) failed on input image"); + } + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("%s", e.what()); + } + catch (cv::Exception& e) + { + ROS_ERROR("%s", e.what()); + } + + // Publish message + publish_fn(compressed); + } + else + ROS_ERROR("Compressed Image Transport - JPEG compression requires 8/16-bit color format (input format is: %s)", message.encoding.c_str()); + + break; + } + // PNG Compression + case PNG: + { + params[0] = CV_IMWRITE_PNG_COMPRESSION; + params[1] = config_.png_level; + + // Update ros message format header + compressed.format += "; png compressed "; + + // Check input format + if ((bitDepth == 8) || (bitDepth == 16)) + { + + // Target image format + stringstream targetFormat; + if (enc::isColor(message.encoding)) + { + // convert color images to RGB domain + targetFormat << "bgr" << bitDepth; + compressed.format += targetFormat.str(); + } + + // OpenCV-ros bridge + try + { + boost::shared_ptr tracked_object; + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat.str()); + + // Compress image + if (cv::imencode(".png", cv_ptr->image, compressed.data, params)) + { + + float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) + / (float)compressed.data.size(); + ROS_DEBUG("Compressed Image Transport - Codec: png, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed.data.size()); + } + else + { + ROS_ERROR("cv::imencode (png) failed on input image"); + } + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("%s", e.what()); + return; + } + catch (cv::Exception& e) + { + ROS_ERROR("%s", e.what()); + return; + } + + // Publish message + publish_fn(compressed); + } + else + ROS_ERROR("Compressed Image Transport - PNG compression requires 8/16-bit encoded color format (input format is: %s)", message.encoding.c_str()); + break; + } + + default: + ROS_ERROR("Unknown compression type '%s', valid options are 'jpeg' and 'png'", config_.format.c_str()); + break; + } + +} + +} //namespace compressed_image_transport diff --git a/ros/image_transport_plugin/compressed_image_transport/src/compressed_subscriber.cpp b/ros/image_transport_plugin/compressed_image_transport/src/compressed_subscriber.cpp new file mode 100644 index 0000000..889f68b --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/src/compressed_subscriber.cpp @@ -0,0 +1,163 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "compressed_image_transport/compressed_subscriber.h" +#include +#include +#include +#include + +#include "compressed_image_transport/compression_common.h" + +#include +#include + +using namespace cv; + +namespace enc = sensor_msgs::image_encodings; + +namespace compressed_image_transport +{ + +void CompressedSubscriber::subscribeImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const Callback& callback, const ros::VoidPtr& tracked_object, + const image_transport::TransportHints& transport_hints) +{ + typedef image_transport::SimpleSubscriberPlugin Base; + Base::subscribeImpl(nh, base_topic, queue_size, callback, tracked_object, transport_hints); + + // Set up reconfigure server for this topic + reconfigure_server_ = boost::make_shared(this->nh()); + ReconfigureServer::CallbackType f = boost::bind(&CompressedSubscriber::configCb, this, _1, _2); + reconfigure_server_->setCallback(f); +} + + +void CompressedSubscriber::configCb(Config& config, uint32_t level) +{ + config_ = config; + if (config_.mode == compressed_image_transport::CompressedSubscriber_gray) { + imdecode_flag_ = cv::IMREAD_GRAYSCALE; + } else if (config_.mode == compressed_image_transport::CompressedSubscriber_color) { + imdecode_flag_ = cv::IMREAD_COLOR; + } else /*if (config_.mode == compressed_image_transport::CompressedSubscriber_unchanged)*/ { + imdecode_flag_ = cv::IMREAD_UNCHANGED; + } +} + + +void CompressedSubscriber::internalCallback(const sensor_msgs::CompressedImageConstPtr& message, + const Callback& user_cb) + +{ + + cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage); + + // Copy message header + cv_ptr->header = message->header; + + // Decode color/mono image + try + { + cv_ptr->image = cv::imdecode(cv::Mat(message->data), imdecode_flag_); + + // Assign image encoding string + const size_t split_pos = message->format.find(';'); + if (split_pos==std::string::npos) + { + // Older version of compressed_image_transport does not signal image format + switch (cv_ptr->image.channels()) + { + case 1: + cv_ptr->encoding = enc::MONO8; + break; + case 3: + cv_ptr->encoding = enc::BGR8; + break; + default: + ROS_ERROR("Unsupported number of channels: %i", cv_ptr->image.channels()); + break; + } + } else + { + std::string image_encoding = message->format.substr(0, split_pos); + + cv_ptr->encoding = image_encoding; + + if ( enc::isColor(image_encoding)) + { + std::string compressed_encoding = message->format.substr(split_pos); + bool compressed_bgr_image = (compressed_encoding.find("compressed bgr") != std::string::npos); + + // Revert color transformation + if (compressed_bgr_image) + { + // if necessary convert colors from bgr to rgb + if ((image_encoding == enc::RGB8) || (image_encoding == enc::RGB16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGB); + + if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGBA); + + if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2BGRA); + } else + { + // if necessary convert colors from rgb to bgr + if ((image_encoding == enc::BGR8) || (image_encoding == enc::BGR16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR); + + if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGRA); + + if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA); + } + } + } + } + catch (cv::Exception& e) + { + ROS_ERROR("%s", e.what()); + } + + size_t rows = cv_ptr->image.rows; + size_t cols = cv_ptr->image.cols; + + if ((rows > 0) && (cols > 0)) + // Publish message to user callback + user_cb(cv_ptr->toImageMsg()); +} + +} //namespace compressed_image_transport diff --git a/ros/image_transport_plugin/compressed_image_transport/src/manifest.cpp b/ros/image_transport_plugin/compressed_image_transport/src/manifest.cpp new file mode 100644 index 0000000..e40e1f1 --- /dev/null +++ b/ros/image_transport_plugin/compressed_image_transport/src/manifest.cpp @@ -0,0 +1,41 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "compressed_image_transport/compressed_publisher.h" +#include "compressed_image_transport/compressed_subscriber.h" + +PLUGINLIB_EXPORT_CLASS( compressed_image_transport::CompressedPublisher, image_transport::PublisherPlugin) + +PLUGINLIB_EXPORT_CLASS( compressed_image_transport::CompressedSubscriber, image_transport::SubscriberPlugin) diff --git a/ros/image_transport_plugin/image_transport_plugins/CHANGELOG.rst b/ros/image_transport_plugin/image_transport_plugins/CHANGELOG.rst new file mode 100644 index 0000000..290d758 --- /dev/null +++ b/ros/image_transport_plugin/image_transport_plugins/CHANGELOG.rst @@ -0,0 +1,107 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package image_transport_plugins +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.9.5 (2016-10-03) +------------------ + +1.9.4 (2016-10-02) +------------------ + +1.9.3 (2016-01-17) +------------------ + +1.9.2 (2015-04-25) +------------------ + +1.9.1 (2014-07-18) +------------------ + +1.9.0 (2014-05-16) +------------------ + +1.8.21 (2013-06-27) +------------------- +* maintainer: david gossow +* buildtool_depend catkin, added metapackage CMakelists.txt +* Contributors: David Gossow + +1.8.20 (2013-03-18) +------------------- +* 1.8.19 -> 1.8.20 +* Contributors: Julius Kammerl + +1.8.19 (2013-02-24) +------------------- +* 1.8.18 -> 1.8.19 +* removing build dependencies from meta package +* Contributors: Julius Kammerl + +1.8.18 (2013-02-07 17:59) +------------------------- +* 1.8.17 -> 1.8.18 +* Contributors: Julius Kammerl + +1.8.17 (2013-01-18) +------------------- +* 1.8.16 -> 1.8.17 +* Contributors: Julius Kammerl + +1.8.16 (2013-01-17) +------------------- +* 1.8.15 -> 1.8.16 +* removing build_tool dependency from meta package +* Contributors: Julius Kammerl + +1.8.15 (2012-12-28 20:11) +------------------------- + +1.8.14 (2012-12-28 20:02) +------------------------- + +1.8.13 (2012-12-28 19:06) +------------------------- + +1.8.12 (2012-12-19 19:30) +------------------------- + +1.8.11 (2012-12-19 17:17) +------------------------- + +1.8.10 (2012-12-19 17:03) +------------------------- + +1.8.9 (2012-12-19 00:26) +------------------------ +* switching to verion 1.8.9 +* Contributors: Julius Kammerl + +1.8.8 (2012-12-17) +------------------ +* adding build_deb on message_generation & mrun_deb on message_runtime +* Updated package.xml for new buildtool_depend tag for catkin requirement +* Contributors: Julius Kammerl, mirzashah + +1.8.7 (2012-12-10 15:29) +------------------------ +* adding missing tf build dependency +* Contributors: Julius Kammerl + +1.8.6 (2012-12-10 15:08) +------------------------ +* switching to version 1.8.6 +* Contributors: Julius Kammerl + +1.8.5 (2012-12-09) +------------------ +* more meta package fixing.. adding build debs +* fixing meta package +* fixed meta package +* switching to 1.8.5 +* Contributors: Julius Kammerl + +1.8.4 (2012-11-30) +------------------ +* switching to version 1.8.4 +* adding image_transport_plugins meta package +* Contributors: Julius Kammerl diff --git a/ros/image_transport_plugin/image_transport_plugins/CMakeLists.txt b/ros/image_transport_plugin/image_transport_plugins/CMakeLists.txt new file mode 100644 index 0000000..7235ebb --- /dev/null +++ b/ros/image_transport_plugin/image_transport_plugins/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(image_transport_plugins) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/ros/image_transport_plugin/image_transport_plugins/package.xml b/ros/image_transport_plugin/image_transport_plugins/package.xml new file mode 100644 index 0000000..a6c6780 --- /dev/null +++ b/ros/image_transport_plugin/image_transport_plugins/package.xml @@ -0,0 +1,28 @@ + + image_transport_plugins + 1.9.5 + + A set of plugins for publishing and subscribing to sensor_msgs/Image topics + in representations other than raw pixel data. For example, for viewing a + stream of images off-robot, a video codec will give much lower bandwidth + and latency. For low frame rate tranport of high-definition images, you + might prefer sending them as JPEG or PNG-compressed form. + + Patrick Mihelich + Julius Kammerl + David Gossow + BSD + + http://www.ros.org/wiki/image_transport_plugins + + catkin + + compressed_depth_image_transport + compressed_image_transport + theora_image_transport + + + + + + diff --git a/ros/image_transport_plugin/theora_image_transport/CHANGELOG.rst b/ros/image_transport_plugin/theora_image_transport/CHANGELOG.rst new file mode 100644 index 0000000..95455bd --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/CHANGELOG.rst @@ -0,0 +1,170 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package theora_image_transport +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.9.5 (2016-10-03) +------------------ + +1.9.4 (2016-10-02) +------------------ +* address gcc6 build error and tune + 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: Lukas Bulwahn + +1.9.3 (2016-01-17) +------------------ +* remove useless tf dependencies +* Contributors: Vincent Rabaud + +1.9.2 (2015-04-25) +------------------ +* get code to compile with OpenCV3 +* Contributors: Vincent Rabaud + +1.9.1 (2014-07-18) +------------------ +* Some cleanup in package.xml and CMakeLists.txt + - builds broke sporadically (I think because of the missing *_gencpp in + add_dependencies) with missing Packet.h file. + - I’m no catkin expert, but these changes make catkin_lint happy (no + more errors at least). +* Contributors: Nikolaus Demmel + +1.9.0 (2014-05-16) +------------------ +* remove __connection_header for indigo see http://github.com/ros-perception/image_transport_plugins.git +* Contributors: Kei Okada + +1.8.21 (2013-06-27) +------------------- + +1.8.20 (2013-03-18) +------------------- +* 1.8.19 -> 1.8.20 +* fixing missing theoraenc and theoradec library links +* Contributors: Julius Kammerl + +1.8.19 (2013-02-24) +------------------- +* 1.8.18 -> 1.8.19 +* Contributors: Julius Kammerl + +1.8.18 (2013-02-07 17:59) +------------------------- +* 1.8.17 -> 1.8.18 +* fixing input format checks (enabling rgba, bgra) + minor fixes +* Contributors: Julius Kammerl + +1.8.17 (2013-01-18) +------------------- +* 1.8.16 -> 1.8.17 +* fixed color conversion bug in theora_image_transport +* Contributors: Julius Kammerl + +1.8.16 (2013-01-17) +------------------- +* 1.8.15 -> 1.8.16 +* use the pluginlib script to remove some runtime warnings +* Contributors: Julius Kammerl, Vincent Rabaud + +1.8.15 (2012-12-28 20:11) +------------------------- + +1.8.14 (2012-12-28 20:02) +------------------------- + +1.8.13 (2012-12-28 19:06) +------------------------- +* fix the bad exports +* make sure the plugins are visible by image_transport +* added license headers to various cpp and h files +* Contributors: Aaron Blasdel, Vincent Rabaud + +1.8.12 (2012-12-19 19:30) +------------------------- +* fix downstream stuff in cmake +* Contributors: Dirk Thomas + +1.8.11 (2012-12-19 17:17) +------------------------- +* fix cmake order +* Contributors: Dirk Thomas + +1.8.10 (2012-12-19 17:03) +------------------------- +* fix dyn reconf +* Contributors: Dirk Thomas + +1.8.9 (2012-12-19 00:26) +------------------------ +* switching to verion 1.8.9 +* fixing dynamic_reconfigure related catkin errors +* Contributors: Julius Kammerl + +1.8.8 (2012-12-17) +------------------ +* more message generation related catkin changes +* adding message_runtime deb to CMakeLists.txt +* adding build_deb on message_generation & mrun_deb on message_runtime +* Updated package.xml for new buildtool_depend tag for catkin requirement +* Contributors: Julius Kammerl, mirzashah + +1.8.7 (2012-12-10 15:29) +------------------------ +* adding missing tf build dependency +* Contributors: Julius Kammerl + +1.8.6 (2012-12-10 15:08) +------------------------ +* switching to version 1.8.6 +* Contributors: Julius Kammerl + +1.8.5 (2012-12-09) +------------------ +* adding missing build debs +* added class_loader_hide_library_symbols macros to CMakeList +* switching to 1.8.5 +* Contributors: Julius Kammerl + +1.8.4 (2012-11-30) +------------------ +* switching to version 1.8.4 +* catkinizing theora_image_transport +* adding plugin.xml exports for pluginlib +* catkinizing theora_image_transport +* github migration from code.ros.org (r40053) +* theora_image_transport: Restored build of ogg_saver, though it really needs more work to be robust. +* theora_image_transport: Removed debug output. +* theora_image_transport: Renamed compressed_plugins.xml to theora_plugins.xml. +* theora_image_transport: Added migration rule for new Packet message. +* image_transport_plugins: Updated manifests to have better summaries, correct URLs. +* theora: Fixed export flags of libtheora. No longer need hack in theora_image_transport's CMakeLists. Temporarily disabled building ogg_saver. +* theora_image_transport: Copy connection header into the output Image. +* theora_image_transport: Publisher sends new headers if image size changes. Better error handling in publisher. Always turn off latching. +* theora_image_transport: Subscriber ignores delta frames until it gets a keyframe. Gets rid of junk frames at the beginning. +* theora_image_transport: Properly clear everything before receiving new headers, which now works without error on the subscriber side. +* theora_image_transport: Better error handling. Support for receiving new headers in subscriber. Handle duplicate frames correctly. Fixed a couple memory leaks. +* theora_image_transport: Force queue_size to be big enough for the headers on both ends. Got rid of sleeps after publishing header packets. More code cleanup. +* theora_image_transport: Added ROS header to Packet msg, fixing `#3882 `_. Fixed reception of comment header and now properly detect when all headers received. +* theora_image_transport: Pull out original (non-padded) region in subscriber. +* theora_image_transport: Cleaned up encoding/decoding to make good use of existing OpenCV functions. Partially fixed `#3082 `_, poor handling of oddly-sized images. +* theora_image_transport: Cleanup of TheoraPublisher. +* Added Ubuntu platform tags to manifest +* Adding ogg_saver node to dump a theora stream to a .ogg file playable in VLC, mplayer, etc +* Fixing bug (typo) where theora_publisher always set target bitrate to 1. I'm surprised it was working at all. +* Remove use of deprecated rosbuild macros +* Switch to opencv2 +* Ooops, segfault +* Hopefully fixed a theora_subscriber bug, Patrick will test. +* theora_image_transport: Override getTransportName(). +* Updating theora_image_transport to work with the latest image_transport API +* Removed explicit library prefix and suffix +* image_transport_plugins: Initial stack check-in. Includes theora_image_transport, compressed_image_transport and libtheora. Currently depends on opencv, but may excise this in the future. +* Contributors: Julius Kammerl, ethan, gerkey, jamesb, mihelich, pmihelich, wheeler diff --git a/ros/image_transport_plugin/theora_image_transport/CMakeLists.txt b/ros/image_transport_plugin/theora_image_transport/CMakeLists.txt new file mode 100644 index 0000000..355149a --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 2.8.3) +project(theora_image_transport) + +find_package(OpenCV REQUIRED) +find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure image_transport message_generation rosbag pluginlib std_msgs) + +add_message_files(DIRECTORY msg FILES Packet.msg) + +generate_messages(DEPENDENCIES std_msgs) + +find_package(PkgConfig) +pkg_check_modules(PC_OGG REQUIRED ogg) +pkg_check_modules(PC_THEORA REQUIRED theora) +pkg_check_modules(PC_THEORAENC REQUIRED theoraenc) +pkg_check_modules(PC_THEORADEC REQUIRED theoradec) + + +# generate the dynamic_reconfigure config file +generate_dynamic_reconfigure_options(cfg/TheoraPublisher.cfg cfg/TheoraSubscriber.cfg) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS message_runtime std_msgs +) + +include_directories(include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${PC_OGG_INCLUDE_DIRS} + ${PC_THEORA_INCLUDE_DIRS} + ${PC_THEORAENC_INCLUDE_DIRS} + ${PC_THEORADEC_INCLUDE_DIRS} +) + +link_directories(${PC_OGG_LIBRARY_DIRS} + ${PC_THEORA_LIBRARY_DIRS} + ${PC_THEORAENC_LIBRARY_DIRS} + ${PC_THEORADEC_LIBRARY_DIRS}) +add_definitions(${PC_OGG_CFLAGS_OTHER} + ${PC_THEORA_CFLAGS_OTHER} + ${PC_THEORAENC_CFLAGS_OTHER} + ${PC_THEORADEC_CFLAGS_OTHER} +) + +add_library(${PROJECT_NAME} src/theora_publisher.cpp src/theora_subscriber.cpp src/manifest.cpp) +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_gencpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + ${PC_OGG_LIBRARIES} + ${PC_THEORA_LIBRARIES} + ${PC_THEORAENC_LIBRARIES} + ${PC_THEORADEC_LIBRARIES} +) + +class_loader_hide_library_symbols(${PROJECT_NAME}) + +add_executable(ogg_saver src/ogg_saver.cpp) +target_link_libraries(ogg_saver ${PC_THEORA_LIBRARY} + ${PC_OGG_LIBRARY} + ${OpenCV_LIBRARIES} + ${catkin_LIBRARIES} + ${PC_THEORAENC_LIBRARIES} + ${PC_THEORADEC_LIBRARIES}) +add_dependencies(ogg_saver ${PROJECT_NAME}_gencpp) + +install(TARGETS ${PROJECT_NAME} ogg_saver + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# add xml file +install(FILES theora_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/ros/image_transport_plugin/theora_image_transport/cfg/TheoraPublisher.cfg b/ros/image_transport_plugin/theora_image_transport/cfg/TheoraPublisher.cfg new file mode 100755 index 0000000..01b1224 --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/cfg/TheoraPublisher.cfg @@ -0,0 +1,18 @@ +#! /usr/bin/env python + +PACKAGE='theora_image_transport' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +optimize_for_enum = gen.enum([ gen.const("Bitrate", int_t, 0, "Aim for requested bitrate"), + gen.const("Quality", int_t, 1, "Aim for requested quality") ], + "Enum to control whether optimizing for bitrate or quality") +gen.add("optimize_for", int_t, 0, "Try to achieve either 'target_bitrate' or 'quality'", 1, 0, 1, + edit_method = optimize_for_enum) +gen.add("target_bitrate", int_t, 0, "Target encoding bitrate, bits per second", 800000, 0, 99200000) +gen.add("quality", int_t, 0, "Encoding quality", 31, 0, 63) +gen.add("keyframe_frequency", int_t, 0, "Maximum distance between key frames", 64, 1, 64) + +exit(gen.generate(PACKAGE, "TheoraPublisher", "TheoraPublisher")) diff --git a/ros/image_transport_plugin/theora_image_transport/cfg/TheoraSubscriber.cfg b/ros/image_transport_plugin/theora_image_transport/cfg/TheoraSubscriber.cfg new file mode 100755 index 0000000..deec68b --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/cfg/TheoraSubscriber.cfg @@ -0,0 +1,11 @@ +#! /usr/bin/env python + +PACKAGE='theora_image_transport' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("post_processing_level", int_t, 0, "Post-processing level. Higher values can improve the appearance of the decoded images at the cost of more CPU.", 0, 0, 7) + +exit(gen.generate(PACKAGE, "TheoraSubscriber", "TheoraSubscriber")) diff --git a/ros/image_transport_plugin/theora_image_transport/include/theora_image_transport/theora_publisher.h b/ros/image_transport_plugin/theora_image_transport/include/theora_image_transport/theora_publisher.h new file mode 100644 index 0000000..6107c7f --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/include/theora_image_transport/theora_publisher.h @@ -0,0 +1,94 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 + +namespace theora_image_transport { + +class TheoraPublisher : public image_transport::SimplePublisherPlugin +{ +public: + TheoraPublisher(); + + ~TheoraPublisher(); + + // Return the system unique string representing the theora transport type + virtual std::string getTransportName() const { return "theora"; } + +protected: + // Overridden to tweak arguments and set up reconfigure server + virtual void advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size, + const image_transport::SubscriberStatusCallback &user_connect_cb, + const image_transport::SubscriberStatusCallback &user_disconnect_cb, + const ros::VoidPtr &tracked_object, bool latch); + + // Callback to send header packets to new clients + virtual void connectCallback(const ros::SingleSubscriberPublisher& pub); + + // Main publish function + virtual void publish(const sensor_msgs::Image& message, + const PublishFn& publish_fn) const; + + // Dynamic reconfigure support + typedef theora_image_transport::TheoraPublisherConfig Config; + typedef dynamic_reconfigure::Server ReconfigureServer; + boost::shared_ptr reconfigure_server_; + + void configCb(Config& config, uint32_t level); + + // Utility functions + bool ensureEncodingContext(const sensor_msgs::Image& image, const PublishFn& publish_fn) const; + void oggPacketToMsg(const std_msgs::Header& header, const ogg_packet &oggpacket, + theora_image_transport::Packet &msg) const; + void updateKeyframeFrequency() const; + + // Some data is preserved across calls to publish(), but from the user's perspective publish() is + // "logically const" + mutable cv_bridge::CvImage img_image_; + mutable th_info encoder_setup_; + mutable ogg_uint32_t keyframe_frequency_; + mutable boost::shared_ptr encoding_context_; + mutable std::vector stream_header_; +}; + +} //namespace compressed_image_transport diff --git a/ros/image_transport_plugin/theora_image_transport/include/theora_image_transport/theora_subscriber.h b/ros/image_transport_plugin/theora_image_transport/include/theora_image_transport/theora_subscriber.h new file mode 100644 index 0000000..43dae13 --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/include/theora_image_transport/theora_subscriber.h @@ -0,0 +1,85 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 + +namespace theora_image_transport { + +class TheoraSubscriber : public image_transport::SimpleSubscriberPlugin +{ +public: + TheoraSubscriber(); + virtual ~TheoraSubscriber(); + + virtual std::string getTransportName() const { return "theora"; } + +protected: + // Overridden to bump queue_size, otherwise we might lose headers + // Overridden to tweak arguments and set up reconfigure server + virtual void subscribeImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size, + const Callback &callback, const ros::VoidPtr &tracked_object, + const image_transport::TransportHints &transport_hints); + + // The function that does the actual decompression and calls a user supplied callback with the resulting image + virtual void internalCallback(const theora_image_transport::PacketConstPtr &msg, const Callback& user_cb); + + // Dynamic reconfigure support + typedef theora_image_transport::TheoraSubscriberConfig Config; + typedef dynamic_reconfigure::Server ReconfigureServer; + boost::shared_ptr reconfigure_server_; + int pplevel_; // Post-processing level + + void configCb(Config& config, uint32_t level); + + // Utility functions + int updatePostProcessingLevel(int level); + void msgToOggPacket(const theora_image_transport::Packet &msg, ogg_packet &ogg); + + bool received_header_; + bool received_keyframe_; + th_dec_ctx* decoding_context_; + th_info header_info_; + th_comment header_comment_; + th_setup_info* setup_info_; + sensor_msgs::ImagePtr latest_image_; +}; + +} //namespace theora_image_transport diff --git a/ros/image_transport_plugin/theora_image_transport/msg/Packet.msg b/ros/image_transport_plugin/theora_image_transport/msg/Packet.msg new file mode 100644 index 0000000..d119312 --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/msg/Packet.msg @@ -0,0 +1,9 @@ +# ROS message adaptation of the ogg_packet struct from libogg, +# see http://www.xiph.org/ogg/doc/libogg/ogg_packet.html. + +Header header # Original sensor_msgs/Image header +uint8[] data # Raw Theora packet data (combines packet and bytes fields from ogg_packet) +int32 b_o_s # Flag indicating whether this packet begins a logical bitstream +int32 e_o_s # Flag indicating whether this packet ends a bitstream +int64 granulepos # A number indicating the position of this packet in the decoded data +int64 packetno # Sequential number of this packet in the ogg bitstream diff --git a/ros/image_transport_plugin/theora_image_transport/package.xml b/ros/image_transport_plugin/theora_image_transport/package.xml new file mode 100644 index 0000000..25ca4d7 --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/package.xml @@ -0,0 +1,40 @@ + + theora_image_transport + 1.9.5 + + Theora_image_transport provides a plugin to image_transport for + transparently sending an image stream encoded with the Theora codec. + + Julius Kammerl + BSD + + http://www.ros.org/wiki/image_transport_plugins + Patrick Mihelich + Ethan Dreyfuss + + catkin + + cv_bridge + dynamic_reconfigure + image_transport + libogg + libtheora + message_generation + pluginlib + rosbag + std_msgs + + cv_bridge + dynamic_reconfigure + image_transport + libogg + libtheora + message_runtime + pluginlib + rosbag + std_msgs + + + + + diff --git a/ros/image_transport_plugin/theora_image_transport/src/manifest.cpp b/ros/image_transport_plugin/theora_image_transport/src/manifest.cpp new file mode 100644 index 0000000..a00e8a6 --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/src/manifest.cpp @@ -0,0 +1,41 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "theora_image_transport/theora_publisher.h" +#include "theora_image_transport/theora_subscriber.h" + +PLUGINLIB_EXPORT_CLASS( theora_image_transport::TheoraPublisher, image_transport::PublisherPlugin) + +PLUGINLIB_EXPORT_CLASS( theora_image_transport::TheoraSubscriber, image_transport::SubscriberPlugin) diff --git a/ros/image_transport_plugin/theora_image_transport/src/ogg_saver.cpp b/ros/image_transport_plugin/theora_image_transport/src/ogg_saver.cpp new file mode 100644 index 0000000..5741aac --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/src/ogg_saver.cpp @@ -0,0 +1,139 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 + +using namespace std; + +class OggSaver +{ +public: + OggSaver(const char* filename) + : fout_(filename, std::ios::out|std::ios::binary) + { + if (ogg_stream_init(&stream_state_, 0) == -1) { + ROS_FATAL("Unable to initialize ogg_stream_state structure"); + exit(1); + } + + sub_ = nh_.subscribe("stream", 10, &OggSaver::processMsg, this); + } + + ~OggSaver() + { + ogg_page page; + if (ogg_stream_flush(&stream_state_, &page) != 0) + writePage(page); + fout_.close(); + ogg_stream_clear(&stream_state_); + } + +private: + + ros::NodeHandle nh_; + ogg_stream_state stream_state_; + ofstream fout_; + ros::Subscriber sub_; + + // When using this caller is responsible for deleting oggpacket.packet!! + void msgToOggPacket(const theora_image_transport::Packet &msg, ogg_packet &oggpacket) + { + oggpacket.bytes = msg.data.size(); + oggpacket.b_o_s = msg.b_o_s; + oggpacket.e_o_s = msg.e_o_s; + oggpacket.granulepos = msg.granulepos; + oggpacket.packetno = msg.packetno; + oggpacket.packet = new unsigned char[oggpacket.bytes]; + memcpy(oggpacket.packet, &msg.data[0], oggpacket.bytes); + } + + void writePage(ogg_page& page) + { + fout_.write((char*)page.header, page.header_len); + fout_.write((char*)page.body, page.body_len); + } + + void processMsg(const theora_image_transport::PacketConstPtr& message) + { + /// @todo Make sure we don't write a video packet first + /// @todo Handle duplicate headers + /// @todo Wait for a keyframe!! + /// @todo Need to flush page for initial identification header packet? And after last header packet? + /// @todo Handle chaining streams? Need to retroactively set e_o_s on previous video packet. + ogg_packet oggpacket; + msgToOggPacket(*message, oggpacket); + boost::scoped_array packet_guard(oggpacket.packet); // Make sure packet memory gets deleted + + if (ogg_stream_packetin(&stream_state_, &oggpacket)) { + ROS_ERROR("Error while adding packet to stream."); + exit(2); + } + + ogg_page page; + if (ogg_stream_pageout(&stream_state_, &page) != 0) + writePage(page); + } +}; + +int main(int argc, char** argv) +{ + /// @todo Use image topic, not stream + /// @todo Option to specify (or figure out?) the frame rate + ros::init(argc, argv, "OggSaver", ros::init_options::AnonymousName); + + if(argc < 2) { + cerr << "Usage: " << argv[0] << " stream:=/theora/image/stream outputFile" << endl; + exit(3); + } + if (ros::names::remap("stream") == "stream") { + ROS_WARN("ogg_saver: stream has not been remapped! Typical command-line usage:\n" + "\t$ ./ogg_saver stream:= outputFile"); + } + + OggSaver saver(argv[1]); + + ros::spin(); + return 0; +} diff --git a/ros/image_transport_plugin/theora_image_transport/src/theora_publisher.cpp b/ros/image_transport_plugin/theora_image_transport/src/theora_publisher.cpp new file mode 100644 index 0000000..2b2c7c4 --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/src/theora_publisher.cpp @@ -0,0 +1,310 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "theora_image_transport/theora_publisher.h" +#include +#include + +#include +#include //for memcpy + +#include + +using namespace std; + +namespace theora_image_transport { + +TheoraPublisher::TheoraPublisher() +{ + // Initialize info structure fields that don't change + th_info_init(&encoder_setup_); + + encoder_setup_.pic_x = 0; + encoder_setup_.pic_y = 0; + encoder_setup_.colorspace = TH_CS_UNSPECIFIED; + encoder_setup_.pixel_fmt = TH_PF_420; // See bottom of http://www.theora.org/doc/libtheora-1.1beta1/codec_8h.html + encoder_setup_.aspect_numerator = 1; + encoder_setup_.aspect_denominator = 1; + encoder_setup_.fps_numerator = 1; // don't know the frame rate ahead of time + encoder_setup_.fps_denominator = 1; + encoder_setup_.keyframe_granule_shift = 6; // A good default for streaming applications + // Note: target_bitrate and quality set to correct values in configCb + encoder_setup_.target_bitrate = -1; + encoder_setup_.quality = -1; +} + +TheoraPublisher::~TheoraPublisher() +{ + th_info_clear(&encoder_setup_); +} + +void TheoraPublisher::advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size, + const image_transport::SubscriberStatusCallback &user_connect_cb, + const image_transport::SubscriberStatusCallback &user_disconnect_cb, + const ros::VoidPtr &tracked_object, bool latch) +{ + // queue_size doesn't account for the 3 header packets, so we correct (with a little extra) here. + queue_size += 4; + // Latching doesn't make a lot of sense with this transport. Could try to save the last keyframe, + // but do you then send all following delta frames too? + latch = false; + typedef image_transport::SimplePublisherPlugin Base; + Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch); + + // Set up reconfigure server for this topic + reconfigure_server_ = boost::make_shared(this->nh()); + ReconfigureServer::CallbackType f = boost::bind(&TheoraPublisher::configCb, this, _1, _2); + reconfigure_server_->setCallback(f); +} + +void TheoraPublisher::configCb(Config& config, uint32_t level) +{ + // target_bitrate must be 0 if we're using quality. + long bitrate = 0; + if (config.optimize_for == theora_image_transport::TheoraPublisher_Bitrate) + bitrate = config.target_bitrate; + bool update_bitrate = bitrate && encoder_setup_.target_bitrate != bitrate; + bool update_quality = !bitrate && ((encoder_setup_.quality != config.quality) || encoder_setup_.target_bitrate > 0); + encoder_setup_.quality = config.quality; + encoder_setup_.target_bitrate = bitrate; + keyframe_frequency_ = config.keyframe_frequency; + + if (encoding_context_) { + int err = 0; + // libtheora 1.1 lets us change quality or bitrate on the fly, 1.0 does not. +#ifdef TH_ENCCTL_SET_BITRATE + if (update_bitrate) { + err = th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_BITRATE, &bitrate, sizeof(long)); + if (err) + ROS_ERROR("Failed to update bitrate dynamically"); + } +#else + err |= update_bitrate; +#endif + +#ifdef TH_ENCCTL_SET_QUALITY + if (update_quality) { + err = th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_QUALITY, &config.quality, sizeof(int)); + // In 1.1 above call will fail if a bitrate has previously been set. That restriction may + // be relaxed in a future version. Complain on other failures. + if (err && err != TH_EINVAL) + ROS_ERROR("Failed to update quality dynamically"); + } +#else + err |= update_quality; +#endif + + // If unable to change parameters dynamically, just create a new encoding context. + if (err) { + encoding_context_.reset(); + } + // Otherwise, do the easy updates and keep going! + else { + updateKeyframeFrequency(); + config.keyframe_frequency = keyframe_frequency_; // In case desired value was unattainable + } + } +} + +void TheoraPublisher::connectCallback(const ros::SingleSubscriberPublisher& pub) +{ + // Send the header packets to new subscribers + for (unsigned int i = 0; i < stream_header_.size(); i++) { + pub.publish(stream_header_[i]); + } +} + +static void cvToTheoraPlane(cv::Mat& mat, th_img_plane& plane) +{ + plane.width = mat.cols; + plane.height = mat.rows; + plane.stride = mat.step; + plane.data = mat.data; +} + +void TheoraPublisher::publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const +{ + if (!ensureEncodingContext(message, publish_fn)) + return; + //return; + /// @todo fromImage is deprecated + /// @todo Optimized gray-scale path, rgb8 + /// @todo fromImage can throw cv::Exception on bayer encoded images + + cv_bridge::CvImageConstPtr cv_image_ptr; + try + { + // conversion necessary + cv_image_ptr = cv_bridge::toCvCopy(message, sensor_msgs::image_encodings::BGR8); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: '%s'", e.what()); + return; + } + catch (cv::Exception& e) + { + ROS_ERROR("OpenCV exception: '%s'", e.what()); + return; + } + + if (cv_image_ptr == 0) { + ROS_ERROR("Unable to convert from '%s' to 'bgr8'", message.encoding.c_str()); + return; + } + + const cv::Mat bgr = cv_image_ptr->image; + + cv::Mat bgr_padded; + int frame_width = encoder_setup_.frame_width, frame_height = encoder_setup_.frame_height; + if (frame_width == bgr.cols && frame_height == bgr.rows) { + bgr_padded = bgr; + } + else { + bgr_padded = cv::Mat::zeros(frame_height, frame_width, bgr.type()); + cv::Mat pic_roi = bgr_padded(cv::Rect(0, 0, bgr.cols, bgr.rows)); + bgr.copyTo(pic_roi); + } + + // Convert image to Y'CbCr color space used by Theora + cv::Mat ycrcb; + cv::cvtColor(bgr_padded, ycrcb, cv::COLOR_BGR2YCrCb); + + // Split channels + cv::Mat ycrcb_planes[3]; + cv::split(ycrcb, ycrcb_planes); + + // Use Y as-is but subsample chroma channels + cv::Mat y = ycrcb_planes[0], cr, cb; + cv::pyrDown(ycrcb_planes[1], cr); + cv::pyrDown(ycrcb_planes[2], cb); + + // Construct Theora image buffer + th_ycbcr_buffer ycbcr_buffer; + cvToTheoraPlane(y, ycbcr_buffer[0]); + cvToTheoraPlane(cb, ycbcr_buffer[1]); + cvToTheoraPlane(cr, ycbcr_buffer[2]); + + // Submit frame to the encoder + int rval = th_encode_ycbcr_in(encoding_context_.get(), ycbcr_buffer); + if (rval == TH_EFAULT) { + ROS_ERROR("[theora] EFAULT in submitting uncompressed frame to encoder"); + return; + } + if (rval == TH_EINVAL) { + ROS_ERROR("[theora] EINVAL in submitting uncompressed frame to encoder"); + return; + } + + // Retrieve and publish encoded video data packets + ogg_packet oggpacket; + theora_image_transport::Packet output; + while ((rval = th_encode_packetout(encoding_context_.get(), 0, &oggpacket)) > 0) { + oggPacketToMsg(message.header, oggpacket, output); + publish_fn(output); + } + if (rval == TH_EFAULT) + ROS_ERROR("[theora] EFAULT in retrieving encoded video data packets"); +} + +void freeContext(th_enc_ctx* context) +{ + if (context) th_encode_free(context); +} + +bool TheoraPublisher::ensureEncodingContext(const sensor_msgs::Image& image, const PublishFn& publish_fn) const +{ + /// @todo Check if encoding has changed + if (encoding_context_ && encoder_setup_.pic_width == image.width && + encoder_setup_.pic_height == image.height) + return true; + + // Theora has a divisible-by-sixteen restriction for the encoded frame size, so + // scale the picture size up to the nearest multiple of 16 and calculate offsets. + encoder_setup_.frame_width = (image.width + 15) & ~0xF; + encoder_setup_.frame_height = (image.height + 15) & ~0xF; + encoder_setup_.pic_width = image.width; + encoder_setup_.pic_height = image.height; + + // Allocate encoding context. Smart pointer ensures that th_encode_free gets called. + encoding_context_.reset(th_encode_alloc(&encoder_setup_), freeContext); + if (!encoding_context_) { + ROS_ERROR("[theora] Failed to create encoding context"); + return false; + } + + updateKeyframeFrequency(); + + th_comment comment; + th_comment_init(&comment); + boost::shared_ptr clear_guard(&comment, th_comment_clear); + /// @todo Store image encoding in comment + comment.vendor = strdup("Willow Garage theora_image_transport"); + + // Construct the header and stream it in case anyone is already listening + /// @todo Try not to send headers twice to some listeners + stream_header_.clear(); + ogg_packet oggpacket; + while (th_encode_flushheader(encoding_context_.get(), &comment, &oggpacket) > 0) { + stream_header_.push_back(theora_image_transport::Packet()); + oggPacketToMsg(image.header, oggpacket, stream_header_.back()); + publish_fn(stream_header_.back()); + } + return true; +} + +void TheoraPublisher::oggPacketToMsg(const std_msgs::Header& header, const ogg_packet &oggpacket, + theora_image_transport::Packet &msg) const +{ + msg.header = header; + msg.b_o_s = oggpacket.b_o_s; + msg.e_o_s = oggpacket.e_o_s; + msg.granulepos = oggpacket.granulepos; + msg.packetno = oggpacket.packetno; + msg.data.resize(oggpacket.bytes); + memcpy(&msg.data[0], oggpacket.packet, oggpacket.bytes); +} + +void TheoraPublisher::updateKeyframeFrequency() const +{ + ogg_uint32_t desired_frequency = keyframe_frequency_; + if (th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_KEYFRAME_FREQUENCY_FORCE, + &keyframe_frequency_, sizeof(ogg_uint32_t))) + ROS_ERROR("Failed to change keyframe frequency"); + if (keyframe_frequency_ != desired_frequency) + ROS_WARN("Couldn't set keyframe frequency to %d, actually set to %d", + desired_frequency, keyframe_frequency_); +} + +} //namespace theora_image_transport diff --git a/ros/image_transport_plugin/theora_image_transport/src/theora_subscriber.cpp b/ros/image_transport_plugin/theora_image_transport/src/theora_subscriber.cpp new file mode 100644 index 0000000..5997f7e --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/src/theora_subscriber.cpp @@ -0,0 +1,243 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, 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 "theora_image_transport/theora_subscriber.h" +#include +#include +#include +#include +#include + +using namespace std; + +namespace theora_image_transport { + +TheoraSubscriber::TheoraSubscriber() + : pplevel_(0), + received_header_(false), + received_keyframe_(false), + decoding_context_(NULL), + setup_info_(NULL) +{ + th_info_init(&header_info_); + th_comment_init(&header_comment_); +} + +TheoraSubscriber::~TheoraSubscriber() +{ + if (decoding_context_) th_decode_free(decoding_context_); + th_setup_free(setup_info_); + th_info_clear(&header_info_); + th_comment_clear(&header_comment_); +} + +void TheoraSubscriber::subscribeImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size, + const Callback &callback, const ros::VoidPtr &tracked_object, + const image_transport::TransportHints &transport_hints) +{ + // queue_size doesn't account for the 3 header packets, so we correct (with a little extra) here. + queue_size += 4; + typedef image_transport::SimpleSubscriberPlugin Base; + Base::subscribeImpl(nh, base_topic, queue_size, callback, tracked_object, transport_hints); + + // Set up reconfigure server for this topic + reconfigure_server_ = boost::make_shared(this->nh()); + ReconfigureServer::CallbackType f = boost::bind(&TheoraSubscriber::configCb, this, _1, _2); + reconfigure_server_->setCallback(f); +} + +void TheoraSubscriber::configCb(Config& config, uint32_t level) +{ + if (decoding_context_ && pplevel_ != config.post_processing_level) { + pplevel_ = updatePostProcessingLevel(config.post_processing_level); + config.post_processing_level = pplevel_; // In case more than PPLEVEL_MAX + } + else + pplevel_ = config.post_processing_level; +} + +int TheoraSubscriber::updatePostProcessingLevel(int level) +{ + int pplevel_max; + int err = th_decode_ctl(decoding_context_, TH_DECCTL_GET_PPLEVEL_MAX, &pplevel_max, sizeof(int)); + if (err) + ROS_WARN("Failed to get maximum post-processing level, error code %d", err); + else if (level > pplevel_max) { + ROS_WARN("Post-processing level %d is above the maximum, clamping to %d", level, pplevel_max); + level = pplevel_max; + } + + err = th_decode_ctl(decoding_context_, TH_DECCTL_SET_PPLEVEL, &level, sizeof(int)); + if (err) { + ROS_ERROR("Failed to set post-processing level, error code %d", err); + return pplevel_; // old value + } + return level; +} + +//When using this caller is responsible for deleting oggpacket.packet!! +void TheoraSubscriber::msgToOggPacket(const theora_image_transport::Packet &msg, ogg_packet &ogg) +{ + ogg.bytes = msg.data.size(); + ogg.b_o_s = msg.b_o_s; + ogg.e_o_s = msg.e_o_s; + ogg.granulepos = msg.granulepos; + ogg.packetno = msg.packetno; + ogg.packet = new unsigned char[ogg.bytes]; + memcpy(ogg.packet, &msg.data[0], ogg.bytes); +} + +void TheoraSubscriber::internalCallback(const theora_image_transport::PacketConstPtr& message, const Callback& callback) +{ + /// @todo Break this function into pieces + ogg_packet oggpacket; + msgToOggPacket(*message, oggpacket); + boost::scoped_array packet_guard(oggpacket.packet); // Make sure packet memory gets deleted + + // Beginning of logical stream flag means we're getting new headers + if (oggpacket.b_o_s == 1) { + // Clear all state, everything we knew is wrong + received_header_ = false; + received_keyframe_ = false; + if (decoding_context_) { + th_decode_free(decoding_context_); + decoding_context_ = NULL; + } + th_setup_free(setup_info_); + setup_info_ = NULL; + th_info_clear(&header_info_); + th_info_init(&header_info_); + th_comment_clear(&header_comment_); + th_comment_init(&header_comment_); + latest_image_.reset(); + } + + // Decode header packets until we get the first video packet + if (received_header_ == false) { + int rval = th_decode_headerin(&header_info_, &header_comment_, &setup_info_, &oggpacket); + switch (rval) { + case 0: + // We've received the full header; this is the first video packet. + decoding_context_ = th_decode_alloc(&header_info_, setup_info_); + if (!decoding_context_) { + ROS_ERROR("[theora] Decoding parameters were invalid"); + return; + } + received_header_ = true; + pplevel_ = updatePostProcessingLevel(pplevel_); + break; // Continue on the video decoding + case TH_EFAULT: + ROS_WARN("[theora] EFAULT when processing header packet"); + return; + case TH_EBADHEADER: + ROS_WARN("[theora] Bad header packet"); + return; + case TH_EVERSION: + ROS_WARN("[theora] Header packet not decodable with this version of libtheora"); + return; + case TH_ENOTFORMAT: + ROS_WARN("[theora] Packet was not a Theora header"); + return; + default: + // If rval > 0, we successfully received a header packet. + if (rval < 0) + ROS_WARN("[theora] Error code %d when processing header packet", rval); + return; + } + } + + // Wait for a keyframe if we haven't received one yet - delta frames are useless to us in that case + received_keyframe_ = received_keyframe_ || (th_packet_iskeyframe(&oggpacket) == 1); + if (!received_keyframe_) + return; + + // We have a video packet we can handle, let's decode it + int rval = th_decode_packetin(decoding_context_, &oggpacket, NULL); + switch (rval) { + case 0: + break; // Yay, we got a frame. Carry on below. + case TH_DUPFRAME: + // Video data hasn't changed, so we update the timestamp and reuse the last received frame. + ROS_DEBUG("[theora] Got a duplicate frame"); + if (latest_image_) { + latest_image_->header = message->header; + callback(latest_image_); + } + return; + case TH_EFAULT: + ROS_WARN("[theora] EFAULT processing video packet"); + return; + case TH_EBADPACKET: + ROS_WARN("[theora] Packet does not contain encoded video data"); + return; + case TH_EIMPL: + ROS_WARN("[theora] The video data uses bitstream features not supported by this version of libtheora"); + return; + default: + ROS_WARN("[theora] Error code %d when decoding video packet", rval); + return; + } + + // We have a new decoded frame available + th_ycbcr_buffer ycbcr_buffer; + th_decode_ycbcr_out(decoding_context_, ycbcr_buffer); + + // Wrap YCbCr channel data into OpenCV format + th_img_plane &y_plane = ycbcr_buffer[0], &cb_plane = ycbcr_buffer[1], &cr_plane = ycbcr_buffer[2]; + cv::Mat y(y_plane.height, y_plane.width, CV_8UC1, y_plane.data, y_plane.stride); + cv::Mat cb_sub(cb_plane.height, cb_plane.width, CV_8UC1, cb_plane.data, cb_plane.stride); + cv::Mat cr_sub(cr_plane.height, cr_plane.width, CV_8UC1, cr_plane.data, cr_plane.stride); + + // Upsample chroma channels + cv::Mat cb, cr; + cv::pyrUp(cb_sub, cb); + cv::pyrUp(cr_sub, cr); + + // Merge into interleaved image. Note OpenCV uses YCrCb, so we swap the chroma channels. + cv::Mat ycrcb, channels[] = {y, cr, cb}; + cv::merge(channels, 3, ycrcb); + + // Convert to BGR color + cv::Mat bgr, bgr_padded; + cv::cvtColor(ycrcb, bgr_padded, CV_YCrCb2BGR); + // Pull out original (non-padded) image region + bgr = bgr_padded(cv::Rect(header_info_.pic_x, header_info_.pic_y, + header_info_.pic_width, header_info_.pic_height)); + + latest_image_ = cv_bridge::CvImage(message->header, sensor_msgs::image_encodings::BGR8, bgr).toImageMsg(); + /// @todo Handle RGB8 or MONO8 efficiently + callback(latest_image_); +} + +} //namespace theora_image_transport diff --git a/ros/image_transport_plugin/theora_image_transport/theora_plugins.xml b/ros/image_transport_plugin/theora_image_transport/theora_plugins.xml new file mode 100644 index 0000000..59018f2 --- /dev/null +++ b/ros/image_transport_plugin/theora_image_transport/theora_plugins.xml @@ -0,0 +1,13 @@ + + + + This plugin publishes a video packet stream encoded using Theora. + + + + + + This plugin decodes a video packet stream encoded using Theora. + + +